Lesson 15: Obstacle Avoidance

Intelligent Navigation with Ultrasonic Detection and Avoidance

🚧 Smart obstacle detection and autonomous navigation behavior

🎯 Learning Objectives

Navigation Skills:

  • • Implement obstacle detection systems
  • • Create intelligent avoidance algorithms
  • • Master autonomous navigation behavior

Programming Concepts:

  • • Distance-based decision making
  • • Mecanum wheel kinematics
  • • Real-time sensor integration

📡 Ultrasonic Sensor Technology

In this lesson, we use a glowing ultrasonic distance sensor module that communicates via the I2C interface. The module retrieves distance measurements from the ultrasonic sensor through I2C communication.

During operation, the module automatically emits eight 40kHz square wave pulses and then listens for an echo. If an echo is detected, it outputs a high signal. The duration of this high-level signal corresponds to the time it takes for the ultrasonic wave to travel to the obstacle and return.

📐 Distance Calculation Formula:

Distance = (High-level duration × Speed of sound (340 m/s)) ÷ 2

🚗 Car Function Description

When the robot car is powered on, it will adjust the color of the glowing ultrasonic module based on the distance to nearby obstacles and will control the car's movement accordingly.

Experiment Setup: Place an obstacle directly in front of the ultrasonic sensor and slowly move it closer.

🌈 (1) Ultrasonic Sensor Color Effects:
  • Distance < 80mm: RGB light enters red breathing mode
  • 80mm ≤ Distance < 180mm: Red gradient, lighter as distance increases
  • 180mm ≤ Distance < 320mm: Blue gradient, intensifying as distance increases
  • 320mm ≤ Distance < 500mm: Green gradient, brighter as distance increases
  • Distance > 500mm: Solid green light
🚗 (2) Car Movement Behavior:
  • Distance ≥ 300mm: The car moves forward
  • Distance < 300mm: The car turns

💡 Tip: Watch how the RGB colors provide visual feedback while the car makes intelligent navigation decisions!

📹 Demo Video: Place your screen-captured 6.6.mp4 in /public/images/ folder

Obstacle avoidance demonstration with intelligent navigation

🚧 Section 6.6: Obstacle Avoidance

Intelligent Navigation System

In this section, you can learn to detect the distance of obstacles through a glowing ultrasonic module. The robot car can be simultaneously controlled to turn right to avoid obstacles. You'll use a glowing ultrasonic sensor to detect the distance of an obstacle and control the car to move forward or turn accordingly.

🎯 Program Flow

  • Ultrasonic Task: Continuously measure distance and set RGB LED colors
  • Motor Control Task: Control car movement based on ultrasonic distance
  • Parallel Processing: Both tasks run simultaneously for real-time response

🚗 Movement Behavior

  • Distance ≥ 300mm: Car moves forward
  • Distance < 300mm: Car turns right to avoid obstacle
  • RGB Feedback: Same color system as previous lessons

⚠️ Safety Reminders

  • Remove Bluetooth module before uploading to avoid serial conflicts
  • Turn off battery box before connecting USB cable
  • Test in open area with adequate space for turning

/**
 * @file ultrasonic_avoid.ino
 * @author Anonymity(Anonymity@hiwonder.com)
 * @brief 超声波避障(Obstacle avoidance)
 * @version V1.0
 * @date 2024-04-23
 *
 * @copyright Copyright (c) 2023
 *
 */

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

// I2C Constants (exact Hiwonder Ultrasound.h definitions)
#define ULTRASOUND_I2C_ADDR         0x77
#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:
  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:
  Ultrasound() {
    Wire.begin();
  }
  
  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);
    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);
    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);
  }
  
  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);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB1_R, &r1, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB1_G, &g1, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB1_B, &b1, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB2_R, &r2, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB2_G, &g2, 1);
    wireWriteDataArray(ULTRASOUND_I2C_ADDR, RGB2_B, &b2, 1);
  }
  
  uint16_t GetDistance() {
    uint16_t distance;
    wireReadDataArray(ULTRASOUND_I2C_ADDR, DISDENCE_L, (uint8_t *)&distance, 2);
    return distance;
  }
};

// OFFICIAL HIWONDER OBSTACLE AVOIDANCE CODE
#define STOP 0
#define FORWARD 1
#define BACKWARD 2
#define FILTER_N 3                ///< 递推平均滤波法(Recursive average filtering method)

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

const static uint8_t keyPin = 3;
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};

bool keyState;          ///< 按键状态检测(Detect button status)
bool taskStart = 0;
uint16_t dis;
uint16_t last_avoid_dis;
uint16_t ultrasonic_distance(void);
int filter_buf[FILTER_N + 1];
int Filter(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);
  Serial.setTimeout(500);
  pinMode(keyPin, INPUT);
  Motor_Init();
  printf("2");
}

void loop() {
  keyState = analogRead(keyPin);
  printf("111");
  if(!keyState) taskStart = 1;
  if(taskStart) {
    ultrasonic_distance();
    dis = ultrasonic_distance();
    Velocity_Controller( 0, 100, 0, 0);
    while(dis < 300) {
      dis = ultrasonic_distance();
      Velocity_Controller( 0, 0, -100, 0);
    }  
  }
}

 /**
 * @brief 滤波(filter)
 * @param filter_sum / FILTER_N
 * @arg None
 * @retval None
 * @note None
 */
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);
}

 /**
 * @brief 超声波距离数据获取(Obtain ultrasonic distance data)
 * @param None
 * @arg None
 * @retval distance
 * @note None
 */
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"); ///< 获取并且串口打印距离，单位mm(Get and print the distance via serial port, unit: 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(void){
  for(uint8_t i = 0; i < 4; i++){
    pinMode(motordirectionPin[i], OUTPUT);
  }
  Velocity_Controller( 0, 0, 0, 0);
}

/**
 * @brief 速度控制函数(Speed control function)
 * @param angle   用于控制小车的运动方向，小车以车头为0度方向，逆时针为正方向。("angle" controls the robot's motion direction, with the front of the robot as 0 degrees and counterclockwise as the positive direction)
 *                取值为0~359(range from 0 to 359)
 * @param velocity   用于控制小车速度，取值为0~100。("velocity" controls the robot's speed, with a value range of 0 to 100)
 * @param rot     用于控制小车的自转速度，取值为-100~100，若大于0小车有一个逆("rot" controls the robot's self-rotation speed, with a value range of -100 to 100)
 *                 时针的自转速度，若小于0则有一个顺时针的自转速度。(If it is greater than 0, the robot has a counterclockwise self-rotation speed. If it is less than 0, the robot has a clockwise self-rotation speed)
 * @param drift   用于决定小车是否开启漂移功能，取值为0或1，若为0则开启，反之关闭。("drift" determines whether the robot enables drift. Value range: 0 or 1. If it is 0, drift is enabled; otherwise, it is disabled)
 * @retval None
 */
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);
}
 
/**
 * @brief PWM与轮子转向设置函数(PWM and wheel turning setting function)
 * @param Motor_x   作为PWM与电机转向的控制数值。根据麦克纳姆轮的运动学分析求得。("Motor_x" is the control value for PWM and motor rotating. Calculated based on the kinematics analysis of mecanum wheels)
 * @retval None
 */
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};///< 前进 左1 右0(Forward; left 1; right 0)
  for(uint8_t i; i < 4; ++i) {
    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]); 
  }
}
← Previous: Lesson 14
Course Overview
Next: Lesson 16 →