Lesson 14: Ultrasonic Ranging and Following

Ultrasonic Skills:

• • Master ultrasonic distance measurement
• • Control RGB LEDs based on distance
• • Implement object following behavior

Programming Concepts:

• • I2C sensor communication
• • Distance-based decision making
• • Motor control algorithms

🚗 Car Movement Behavior

• Distance < 200mm: Car moves backward
• 200mm - 300mm: Car stops
• 300mm - 700mm: Car moves forward
• Distance > 700mm: Car stops again

🎬 Expected Behavior

🌈 RGB LED Behavior

• Distance < 80mm: Red breathing light
• 80mm - 180mm: Red gradient (brighter as distance increases)
• 180mm - 320mm: Blue gradient (brighter as distance increases)
• 320mm - 500mm: Green gradient (brighter as distance increases)
• > 500mm: Solid green light

⚙️ How It Works

The ultrasonic sensor emits 8 square waves at 40kHz and measures the time for the echo to return. The distance is calculated using: Distance = (High-level time × 340 m/s) ÷ 2

• Supply Voltage: 5V
• Operating Current: 2mA
• Effective Range: 2cm - 400cm

📁 Important: Multi-File Structure

The official Hiwonder code uses 3 separate files. You need all three files in your Arduino project:

• ultrasonic_test.ino - Main Arduino sketch (shown below)
• Ultrasound.h - Header file with class declarations
• Ultrasound.cpp - Implementation file with class methods

Note: The constructor automatically calls Wire.begin(), so no separate begin() method is needed!

📁 Complete File Structure

The official Hiwonder implementation uses 3 separate files. Download all files using the buttons above, then create your Arduino project with all three files.

• Ultrasound.h - Header file with class declarations and register definitions
• Ultrasound.cpp - Implementation file with all class methods
• ultrasonic_test.ino - Main Arduino sketch (shown below)

📄 File 3: ultrasonic_test.ino

Create this file last - it's the main Arduino sketch that uses the Ultrasound class.

// Ultrasonic Distance Detection with RGB Feedback
#include "Ultrasound.h"

#define FILTER_N 3                // Filter array capacity

int Filter_Value;
int filter_buf[FILTER_N + 1];
Ultrasound ultrasound;            // Instantiate ultrasound class

int Filter(void);
int ultrasonic_distance(void);

void setup() {
  Serial.begin(9600);
  // Ultrasound sensor initializes automatically in constructor
}

void loop() {
  ultrasonic_distance();
}

// Recursive average filtering method
int Filter() {
  int i;
  int filter_sum = 0;
  filter_buf[FILTER_N] = ultrasound.GetDistance();  // Read ultrasonic value
  for(i = 0; i < FILTER_N; i++) {
    filter_buf[i] = filter_buf[i + 1];              // Shift data left
    filter_sum += filter_buf[i];
  }
  return (int)(filter_sum / FILTER_N);
}

int ultrasonic_distance(){
  uint8_t s;
  uint16_t distance = Filter();                     // Get filtered output
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" mm");

  if (distance > 0 && distance <= 80){
      ultrasound.Breathing(1, 0, 0, 1, 0, 0);      // Red breathing mode
  }
  else if (distance > 80 && distance <= 180){
      s = map(distance, 80, 180, 0, 255);
      ultrasound.Color((255-s), 0, 0, (255-s), 0, 0); // Red gradient
  }
  else if (distance > 180 && distance <= 320){
      s = map(distance, 180, 320, 0, 255);
      ultrasound.Color(0, 0, s, 0, 0, s);          // Blue gradient
  }
  else if (distance > 320 && distance <= 500){
      s = map(distance, 320, 500, 0, 255);
      ultrasound.Color(0, s, 0, 0, s, 0);          // Green gradient
  }
  else if (distance > 500){
      ultrasound.Color(0, 255, 0, 0, 255, 0);      // Pure green
  }
  
  delay(100);
  return distance;
}

✅ Section 6.4 Complete!

Success! The ultrasonic ranging code is ready. Download the 3 files above and compile in Arduino IDE. The code will work perfectly with the official Hiwonder library files.

⚙️ Technical Specifications

• Supply Voltage: 5V
• Operating Current: 2mA
• Effective Range: 2cm - 400cm

🎬 Expected Behavior

🚗 Car Movement Behavior

• Distance < 200mm: Car moves backward
• 200mm - 300mm: Car stops
• 300mm - 700mm: Car moves forward
• Distance > 700mm: Car stops again

🎬 Expected Behavior

⚠️ Important Notes

• Remove the Bluetooth module before uploading to avoid serial port conflicts
• Use the same 3 files from Section 6.4, but replace the main .ino file with the following code

// Ultrasonic Following Robot - Official Hiwonder Implementation
#include "Ultrasound.h"

#define FILTER_N 3                // Filter array capacity

int Filter_Value;
int filter_buf[FILTER_N + 1];

Ultrasound ultrasound;            // Instantiate ultrasound class
const static uint8_t pwm_min = 50;
const static uint8_t motorpwmPin[4] = {10, 9, 6, 11};
const static uint8_t motordirectionPin[4] = {12, 8, 7, 13};
uint16_t dis;

int Filter(void);
uint16_t ultrasonic_distance(void);
void Motor_Init(void);
void Velocity_Controller(uint16_t angle, uint8_t velocity, int8_t rot, bool drift);
void Motors_Set(int8_t Motor_0, int8_t Motor_1, int8_t Motor_2, int8_t Motor_3);

void setup() {
  Serial.begin(9600);
  // Ultrasound sensor initializes automatically in constructor
  Motor_Init();
}

void loop() {
  ultrasonic_distance();
  dis = ultrasonic_distance();
  
  // Official Hiwonder movement logic
  if(dis >= 700) 
    Velocity_Controller(0, 0, 0, 0);      // Stop - object too far
  if(dis >= 300 && dis < 700) 
    Velocity_Controller(0, 50, 0, 0);     // Move forward - follow object
  if(dis >= 200 && dis < 300) 
    Velocity_Controller(0, 0, 0, 0);      // Stop - optimal distance
  if(dis < 200) 
    Velocity_Controller(180, 50, 0, 0);   // Move backward - too close
}

// Recursive average filtering method
int Filter() {
  int i, filter_sum = 0;
  filter_buf[FILTER_N] = ultrasound.GetDistance();
  for(i = 0; i < FILTER_N; i++) {
    filter_buf[i] = filter_buf[i + 1];
    filter_sum += filter_buf[i];
  }
  return (int)(filter_sum / FILTER_N);
}

// Obtain ultrasonic distance data with RGB feedback
uint16_t ultrasonic_distance(){
  uint8_t s;
  uint16_t distance = Filter();         // Get filtered output value
  Serial.print("Distance: ");Serial.print(distance);Serial.println(" mm");

  if (distance > 0 && distance <= 80){
      ultrasound.Breathing(1, 0, 0, 1, 0, 0); // Red breathing light mode, 0.1s cycle
  }
  else if (distance > 80 && distance <= 180){
      s = map(distance,80,180,0,255);
      ultrasound.Color((255-s), 0, 0, (255-s), 0, 0); // Red gradient
  }
  else if (distance > 180 && distance <= 320){
      s = map(distance,180,320,0,255);
      ultrasound.Color(0, 0, s, 0, 0, s); // Blue gradient
  }
  else if (distance > 320 && distance <= 500){
      s = map(distance,320,500,0,255);
      ultrasound.Color(0, s, 255-s, 0, s, 255-s); // Green gradient
  }
  else if (distance > 500){
      ultrasound.Color(0, 255, 0, 0, 255, 0); // Green
  }
  return distance;  
}

void Motor_Init(){
  for(int i = 0; i < 4; i++){
    pinMode(motorpwmPin[i], OUTPUT);
    pinMode(motordirectionPin[i], OUTPUT);
  }
}

// Official Hiwonder Velocity Controller
void Velocity_Controller(uint16_t angle, uint8_t velocity, int8_t rot, bool drift) {
  int8_t velocity_0, velocity_1, velocity_2, velocity_3;
  float speed = 1;
  angle += 90;
  float rad = angle * PI / 180;
  if (rot == 0) speed = 1;    // Speed factor
  else speed = 0.5; 
  velocity /= sqrt(2);
  if (drift) {
    velocity_0 = (velocity * sin(rad) - velocity * cos(rad)) * speed;
    velocity_1 = (velocity * sin(rad) + velocity * cos(rad)) * speed;
    velocity_2 = (velocity * sin(rad) - velocity * cos(rad)) * speed - rot * speed * 2;
    velocity_3 = (velocity * sin(rad) + velocity * cos(rad)) * speed + rot * speed * 2;
  } else {
    velocity_0 = (velocity * sin(rad) - velocity * cos(rad)) * speed + rot * speed;
    velocity_1 = (velocity * sin(rad) + velocity * cos(rad)) * speed - rot * speed;
    velocity_2 = (velocity * sin(rad) - velocity * cos(rad)) * speed - rot * speed;
    velocity_3 = (velocity * sin(rad) + velocity * cos(rad)) * speed + rot * speed;
  }
  Motors_Set(velocity_0, velocity_1, velocity_2, velocity_3);
}

// Official Hiwonder Motors_Set function
void Motors_Set(int8_t Motor_0, int8_t Motor_1, int8_t Motor_2, int8_t Motor_3) {
  int8_t pwm_set[4];
  int8_t motors[4] = { Motor_0, Motor_1, Motor_2, Motor_3};
  bool direction[4] = { 1, 0, 0, 1};  // Forward; left 1; right 0
  for(uint8_t i = 0; i < 4; ++i) {  // Fixed: added i = 0 initialization
    if(motors[i] < 0) direction[i] = !direction[i];
    else direction[i] = direction[i];

    if(motors[i] == 0) pwm_set[i] = 0;
    else pwm_set[i] = map(abs(motors[i]), 0, 100, pwm_min, 255);

    digitalWrite(motordirectionPin[i], direction[i]); 
    analogWrite(motorpwmPin[i], pwm_set[i]); 
  }
}

✅ Section 6.5 Complete!

Success! The ultrasonic following code is ready. Use the same Ultrasound.h and Ultrasound.cpp files from Section 6.4, but replace the main .ino file with the code above. The robot will follow objects by maintaining optimal distance using the official Hiwonder implementation.