Lesson 14: Ultrasonic Ranging and Following

Distance Detection with RGB Feedback and Object Following

📡 Advanced ultrasonic sensor programming with intelligent following behavior

🎯 Learning Objectives

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

📡 Section 6.4: Ultrasonic Ranging

Distance Detection with RGB Feedback

In this lesson, you'll learn how to use a glowing ultrasonic sensor to detect the distance of nearby obstacles. You'll also control the color of the sensor's built-in RGB LED based on the measured distance.

🌈 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

/**
 * @file ultrasonic_test.ino
 * @brief 超声波距离测试(Ultrasonic ranging)
 * @author Hiwonder (adapted for standard Arduino)
 * @version V1.0
 * @date 2024-04-23
 */

// Compatible Ultrasound class - matches Hiwonder I2C implementation
#include <Wire.h>

// I2C Constants (exact Hiwonder Ultrasound.h definitions)
#define ULTRASOUND_I2C_ADDR         0x77

// Register Map
#define DISDENCE_L                  0    // Distance low 8 bits, unit: mm
#define DISDENCE_H                  1    // Distance high 8 bits

#define RGB_BRIGHTNESS              50   // 0-255
#define RGB_WORK_MODE               2    // RGB mode: 0=user defined, 1=breathing mode

#define RGB1_R                      3    // Probe 1 R value, 0~255
#define RGB1_G                      4    // Probe 1 G value, 0~255  
#define RGB1_B                      5    // Probe 1 B value, 0~255

#define RGB2_R                      6    // Probe 2 R value, 0~255
#define RGB2_G                      7    // Probe 2 G value, 0~255
#define RGB2_B                      8    // Probe 2 B value, 0~255

#define RGB1_R_BREATHING_CYCLE      9    // Breathing cycle for probe 1 R, unit: 100ms
#define RGB1_G_BREATHING_CYCLE      10   // Breathing cycle for probe 1 G, unit: 100ms
#define RGB1_B_BREATHING_CYCLE      11   // Breathing cycle for probe 1 B, unit: 100ms

#define RGB2_R_BREATHING_CYCLE      12   // Breathing cycle for probe 2 R, unit: 100ms
#define RGB2_G_BREATHING_CYCLE      13   // Breathing cycle for probe 2 G, unit: 100ms
#define RGB2_B_BREATHING_CYCLE      14   // Breathing cycle for probe 2 B, unit: 100ms

#define RGB_WORK_SIMPLE_MODE        0
#define RGB_WORK_BREATHING_MODE     1

class Ultrasound {
private:
  // I2C communication methods (matching Hiwonder implementation)
  bool wireWriteByte(uint8_t addr, uint8_t val) {
    Wire.beginTransmission(addr);
    Wire.write(val);
    return (Wire.endTransmission() == 0);
  }
  
  bool wireWriteDataArray(uint8_t addr, uint8_t reg, uint8_t *val, unsigned int len) {
    Wire.beginTransmission(addr);
    Wire.write(reg);
    for(unsigned int i = 0; i < len; i++) {
      Wire.write(val[i]);
    }
    return (Wire.endTransmission() == 0);
  }
  
  int wireReadDataArray(uint8_t addr, uint8_t reg, uint8_t *val, unsigned int len) {
    if (!wireWriteByte(addr, reg)) return -1;
    Wire.requestFrom(addr, len);
    unsigned char i = 0;
    while (Wire.available() && i < len) {
      val[i] = Wire.read();
      i++;
    }
    return i;
  }
  
public:
  // Constructor - initialize I2C (matches Hiwonder)
  Ultrasound() {
    Wire.begin();
  }
  
  // RGB breathing mode (exact Hiwonder implementation)
  void Breathing(uint8_t r1, uint8_t g1, uint8_t b1, uint8_t r2, uint8_t g2, uint8_t b2) {
    uint8_t value = RGB_WORK_BREATHING_MODE;
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB_WORK_MODE, &value, 1);
    
    // Set breathing cycles for RGB1 (probe 1)
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB1_R_BREATHING_CYCLE, &r1, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB1_G_BREATHING_CYCLE, &g1, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB1_B_BREATHING_CYCLE, &b1, 1);
    
    // Set breathing cycles for RGB2 (probe 2)  
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB2_R_BREATHING_CYCLE, &r2, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB2_G_BREATHING_CYCLE, &g2, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB2_B_BREATHING_CYCLE, &b2, 1);
  }
  
  // RGB color control (exact Hiwonder implementation)
  void Color(uint8_t r1, uint8_t g1, uint8_t b1, uint8_t r2, uint8_t g2, uint8_t b2) {
    uint8_t value = RGB_WORK_SIMPLE_MODE;
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB_WORK_MODE, &value, 1);
    
    // Set RGB1 (probe 1) colors
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB1_R, &r1, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB1_G, &g1, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB1_B, &b1, 1);
    
    // Set RGB2 (probe 2) colors
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB2_R, &r2, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB2_G, &g2, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB2_B, &b2, 1);
  }
  
  // Distance measurement (exact Hiwonder implementation)
  uint16_t GetDistance() {
    uint16_t distance;
    wireReadDataArray(ULTRASOUND_I2C_ADDR, DISDENCE_L, (uint8_t *)&distance, 2);
    return distance;
  }
};

// OFFICIAL HIWONDER CODE (with compatible library)
#define FILTER_N 3                ///< 滤波法数组容量(Filter array capacity)

int Filter_Value;
int filter_buf[FILTER_N + 1];

Ultrasound ultrasound;            ///< 实例化超声波类(Instantiate the ultrasound class)

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

void setup() {
  Serial.begin(9600);
  ultrasound.begin();             // Initialize ultrasound sensor
}

void loop() {
  ultrasonic_distance();
}

/* 递推平均滤波法(Recursive average filtering method)*/
int Filter() {
  int i;
  int filter_sum = 0;
  filter_buf[FILTER_N] = ultrasound.GetDistance();///< 读取超声波测值(Read the ultrasonic ranging value)
  for(i = 0; i < FILTER_N; i++) {
    filter_buf[i] = filter_buf[i + 1];///< 所有数据左移，低位仍掉(Shift all data to the left, and clear the low bit)
    filter_sum += filter_buf[i];
  }
  return (int)(filter_sum / FILTER_N);
}

int ultrasonic_distance(){
  uint8_t s;
  uint16_t distance = Filter();///< 获得滤波器输出值(Get the output value of the filter)
  Serial.print("Distance: ");///< 获取并且串口打印距离，单位mm(Get and print the distance via serial port, unit: mm)
  Serial.print(distance);
  Serial.println(" mm");

  if (distance > 0 && distance <= 80){
      ultrasound.Breathing(1, 0, 0, 1, 0, 0);///< 呼吸灯模式，周期0.1s，颜色红色(Red breathing light mode, in 0.1s)
  }
   
  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;  
}

🤖 Section 6.5: Ultrasonic Following

Object Following Behavior

In this section, you'll learn how to use a glowing ultrasonic module to detect obstacle distances and control a robot car to follow the movement of an object.

🚗 Car Movement Behavior

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

⚠️ Important Notes

Remove the Bluetooth module before uploading to avoid serial port conflicts
Turn off the battery box before connecting USB cable
Ensure proper motor connections before testing

/**
 * @file ultrasonic_following.ino
 * @brief 超声波跟随(Ultrasonic following)
 * @author Hiwonder (adapted for standard Arduino)
 * @version V1.0
 * @date 2024-04-23
 */

// Compatible Ultrasound class - matches Hiwonder I2C implementation
#include <Wire.h>

// I2C Constants (exact Hiwonder Ultrasound.h definitions)
#define ULTRASOUND_I2C_ADDR         0x77

// Register Map
#define DISDENCE_L                  0    // Distance low 8 bits, unit: mm
#define DISDENCE_H                  1    // Distance high 8 bits

#define RGB_BRIGHTNESS              50   // 0-255
#define RGB_WORK_MODE               2    // RGB mode: 0=user defined, 1=breathing mode

#define RGB1_R                      3    // Probe 1 R value, 0~255
#define RGB1_G                      4    // Probe 1 G value, 0~255  
#define RGB1_B                      5    // Probe 1 B value, 0~255

#define RGB2_R                      6    // Probe 2 R value, 0~255
#define RGB2_G                      7    // Probe 2 G value, 0~255
#define RGB2_B                      8    // Probe 2 B value, 0~255

#define RGB1_R_BREATHING_CYCLE      9    // Breathing cycle for probe 1 R, unit: 100ms
#define RGB1_G_BREATHING_CYCLE      10   // Breathing cycle for probe 1 G, unit: 100ms
#define RGB1_B_BREATHING_CYCLE      11   // Breathing cycle for probe 1 B, unit: 100ms

#define RGB2_R_BREATHING_CYCLE      12   // Breathing cycle for probe 2 R, unit: 100ms
#define RGB2_G_BREATHING_CYCLE      13   // Breathing cycle for probe 2 G, unit: 100ms
#define RGB2_B_BREATHING_CYCLE      14   // Breathing cycle for probe 2 B, unit: 100ms

#define RGB_WORK_SIMPLE_MODE        0
#define RGB_WORK_BREATHING_MODE     1

class Ultrasound {
private:
  // I2C communication methods (exact Hiwonder implementation)
  bool wireWriteByte(uint8_t addr, uint8_t val) {
    Wire.beginTransmission(addr);
    Wire.write(val);
    return (Wire.endTransmission() == 0);
  }
  
  bool wireWriteDataArray(uint8_t addr, uint8_t reg, uint8_t *val, unsigned int len) {
    Wire.beginTransmission(addr);
    Wire.write(reg);
    for(unsigned int i = 0; i < len; i++) {
      Wire.write(val[i]);
    }
    return (Wire.endTransmission() == 0);
  }
  
  int wireReadDataArray(uint8_t addr, uint8_t reg, uint8_t *val, unsigned int len) {
    if (!wireWriteByte(addr, reg)) return -1;
    Wire.requestFrom(addr, len);
    unsigned char i = 0;
    while (Wire.available() && i < len) {
      val[i] = Wire.read();
      i++;
    }
    return i;
  }
  
public:
  // Constructor - initialize I2C (matches Hiwonder)
  Ultrasound() {
    Wire.begin();
  }
  
  // RGB breathing mode (exact Hiwonder implementation)
  void Breathing(uint8_t r1, uint8_t g1, uint8_t b1, uint8_t r2, uint8_t g2, uint8_t b2) {
    uint8_t value = RGB_WORK_BREATHING_MODE;
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB_WORK_MODE, &value, 1);
    
    // Set breathing cycles for RGB1 (probe 1)
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB1_R_BREATHING_CYCLE, &r1, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB1_G_BREATHING_CYCLE, &g1, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB1_B_BREATHING_CYCLE, &b1, 1);
    
    // Set breathing cycles for RGB2 (probe 2)  
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB2_R_BREATHING_CYCLE, &r2, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB2_G_BREATHING_CYCLE, &g2, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB2_B_BREATHING_CYCLE, &b2, 1);
  }
  
  // RGB color control (exact Hiwonder implementation)
  void Color(uint8_t r1, uint8_t g1, uint8_t b1, uint8_t r2, uint8_t g2, uint8_t b2) {
    uint8_t value = RGB_WORK_SIMPLE_MODE;
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB_WORK_MODE, &value, 1);
    
    // Set RGB1 (probe 1) colors
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB1_R, &r1, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB1_G, &g1, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB1_B, &b1, 1);
    
    // Set RGB2 (probe 2) colors
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB2_R, &r2, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB2_G, &g2, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB2_B, &b2, 1);
  }
  
  // Distance measurement (exact Hiwonder implementation)
  uint16_t GetDistance() {
    uint16_t distance;
    wireReadDataArray(ULTRASOUND_I2C_ADDR, DISDENCE_L, (uint8_t *)&distance, 2);
    return distance;
  }
};

// OFFICIAL HIWONDER ULTRASONIC FOLLOWING CODE
#define FILTER_N 3                ///< 滤波法数组容量(Filter array capacity)

int Filter_Value;
int filter_buf[FILTER_N + 1];

Ultrasound ultrasound;            ///< 实例化超声波类(Instantiate the 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 setup() {
  Serial.begin(9600);
  ultrasound.begin();
  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) */
uint16_t ultrasonic_distance(){
  uint8_t s;
  uint16_t distance = Filter(); // 获得滤波器输出值(Get the output value of the filter)
  Serial.print("Distance: ");Serial.print(distance);Serial.println(" mm");

  if (distance > 0 && distance <= 80){
      ultrasound.Breathing(1, 0, 0, 1, 0, 0); //呼吸灯模式，周期0.1s，颜色红色(Red breathing light mode, in 0.1s)
  }
  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);
  }
}

// Simplified Velocity Controller (students can expand based on their setup)
void Velocity_Controller(uint16_t angle, uint8_t velocity, int8_t rot, bool drift){
  // Basic mecanum wheel control implementation
  // angle: movement direction (0° = forward)
  // velocity: movement speed (0-100)
  // rot: rotation (-100 to +100)
  // drift: enable/disable drift mode
  
  if(velocity > 0 && rot == 0){
    // Move forward/backward
    for(int i = 0; i < 4; i++){
      analogWrite(motorpwmPin[i], map(velocity, 0, 100, pwm_min, 255));
      digitalWrite(motordirectionPin[i], angle == 0 ? HIGH : LOW);
    }
  } else if(rot != 0){
    // Rotate in place
    for(int i = 0; i < 4; i++){
      analogWrite(motorpwmPin[i], map(abs(rot), 0, 100, pwm_min, 255));
      digitalWrite(motordirectionPin[i], (i < 2) ? (rot > 0) : (rot < 0));
    }
  } else {
    // Stop all motors
    for(int i = 0; i < 4; i++){
      analogWrite(motorpwmPin[i], 0);
    }
  }
}

← Previous: Lesson 13

Course Overview

Next: Lesson 15 →