Lesson 16: Line Following

In this lesson, we will use a 4-channel line follower that utilizes an I2C communication interface to read data from the sensor probes.

The sensor module consists of four probes, each equipped with an infrared emitter and an infrared receiver. The infrared light is strongly reflected by white surfaces and weakly reflected by black surfaces. This variation in reflection allows the sensor to detect whether the line is present.

🔧 4-Channel Sensor Array Layout

Line following demonstration with 4-channel sensor array

Complete Line Following Implementation

The complete Hiwonder line following code with I2C sensor communication, motor control, and all helper functions in one comprehensive implementation.

/**
 * @file tracking_test.ino
 * @author Hiwonder
 * @brief Line following with 4-channel I2C sensor
 * @version V1.0
 * @date 2024-04-23
 */

#include <Wire.h>
#include "FastLED.h"

/* I2C address of the line follower sensor */
#define LINE_FOLLOWER_I2C_ADDR 0x78 

const static uint8_t ledPin = 2;
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 };
const static uint8_t TRACKING = 4;

static CRGB rgbs[1];
static uint8_t modestate = TRACKING;

uint8_t state = 0;
uint8_t data;
uint8_t rec_data[4];  // Individual sensor readings

bool keyState;
bool taskStart = 0;

// Function prototypes
bool WireWriteByte(uint8_t val);
bool WireReadDataByte(uint8_t reg, uint8_t &val);
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 Sensor_Receive(void);
void Tracking_Line_Task(void);
void Task_Dispatcher(void);
void Rgb_Show(uint8_t rValue, uint8_t gValue, uint8_t bValue);

void setup() {
  Serial.begin(9600);
  
  pinMode(keyPin, INPUT);
  FastLED.addLeds<WS2812, ledPin, GRB>(rgbs, 1);
  Rgb_Show(255, 255, 255);  // White LED initially
  
  Wire.begin();
  Motor_Init();
}

void loop() {
  keyState = analogRead(keyPin);
  if (!keyState) taskStart = 1;  // Start when button pressed
  
  if (taskStart) {
    Sensor_Receive();
    Task_Dispatcher();
  }
}

/* I2C Communication Functions */
bool WireWriteByte(uint8_t val) {
  Wire.beginTransmission(LINE_FOLLOWER_I2C_ADDR);
  Wire.write(val);
  if (Wire.endTransmission() != 0) {
    return false;  // Send failed
  }
  return true;     // Send successful
}

bool WireReadDataByte(uint8_t reg, uint8_t &val) {
  if (!WireWriteByte(reg)) {
    return false;
  }
  Wire.requestFrom(LINE_FOLLOWER_I2C_ADDR, 1);
  while (Wire.available()) {
    val = Wire.read();
  }
  return true;
}

/* Get sensor data from I2C */
void Sensor_Receive(void) {
  WireReadDataByte(1, data);
  rec_data[0] = data & 0x01;        // Sensor 0 (rightmost)
  rec_data[1] = (data >> 1) & 0x01; // Sensor 1 
  rec_data[2] = (data >> 2) & 0x01; // Sensor 2
  rec_data[3] = (data >> 3) & 0x01; // Sensor 3 (leftmost)
}

/* Line following algorithm */
void Tracking_Line_Task(void) {
  Rgb_Show(255, 0, 0);  // Red LED during tracking
  
  if (rec_data[1] == 1 && rec_data[2] == 1) {
    // Both center sensors detect line - go straight
    Velocity_Controller(0, 80, 0, 0);
  }
  else if (rec_data[1] == 1 && rec_data[2] == 0) {
    // Left center sensor detects line - turn left
    Velocity_Controller(0, 80, 65, 0);
  }
  else if (rec_data[1] == 0 && rec_data[2] == 1) {
    // Right center sensor detects line - turn right
    Velocity_Controller(0, 80, -65, 0);
  }
  
  // If no center sensors detect line, stop and wait
  while (rec_data[1] == 0 && rec_data[2] == 0) {
    Sensor_Receive();
    Velocity_Controller(0, 0, 0, 0);  // Stop
  }
}

/* Motor initialization */
void Motor_Init(void) {
  for (uint8_t i = 0; i < 4; i++) {
    pinMode(motordirectionPin[i], OUTPUT);
  }
  Velocity_Controller(0, 0, 0, 0);  // Stop all motors initially
}

/* Mecanum wheel 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;    // Normal speed
  else speed = 0.5;           // Reduced speed when rotating
  
  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);
}

/* Set individual motor speeds and directions */
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 direction settings
  
  for (uint8_t i = 0; i < 4; ++i) {
    if (motors[i] < 0) 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]);
  }
}

/* Task dispatcher */
void Task_Dispatcher() {
  switch (modestate) {
    case TRACKING:
      Tracking_Line_Task();
      break;
  }
}

/* RGB LED control */
void Rgb_Show(uint8_t rValue, uint8_t gValue, uint8_t bValue) {
  rgbs[0].r = rValue;
  rgbs[0].g = gValue;
  rgbs[0].b = bValue;
  FastLED.show();
}
                    

        /**
 * @file tracking_avoid.ino
 * @author Anonymity(Anonymity@hiwonder.com)
 * @brief 巡线和避障
 * @version V1.0
 * @date 2024-04-23
 *
 * @copyright Copyright (c) 2023
 *
 */

#include <Wire.h>
#include "Ultrasound.h"
#include "FastLED.h"

#define LINE_FOLLOWER_I2C_ADDR 0x78/* 寻线传感器的iic地址 */ 
#define FILTER_N 3                //递推平均滤波法

Ultrasound ultrasound;  //实例化超声波类

const static uint8_t ledPin = 2;
const static uint8_t keyPin = 3;
const static uint8_t buzzerPin = 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};
const static uint8_t TRACKING = 4;
const static uint8_t AVOID = 6;

static CRGB rgbs[1];
static uint8_t modestate = TRACKING;

uint8_t state = 0;
uint8_t data;
uint8_t rec_data[4];
uint8_t last_data[2];
uint16_t dis;
int filter_buf[FILTER_N + 1];

bool keyState;          ///< 按键状态检测
bool taskStart = 0;
bool WireWriteByte(uint8_t val);
bool WireReadDataByte(uint8_t reg, uint8_t &val);

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 Sensor_Receive(void);
void Tracking_Line_Task(void);
void Avoid_Task(void);
void Task_Dispatcher(void);
int Filter(void);
uint16_t ultrasonic_distance(void);

void setup(){
  /* 配置通信 */
  Serial.begin(9600);

  pinMode(keyPin, INPUT);
  FastLED.addLeds<WS2812, ledPin, GRB>(rgbs, 1);
  Rgb_Show(255,255,255);
  
  Wire.begin();
  Motor_Init();
  
}

void loop(){
  keyState = analogRead(keyPin);
  if(!keyState) taskStart = 1;
  if(taskStart) {
    Sensor_Receive();
    ultrasonic_distance();
    dis = ultrasonic_distance();
    Task_Dispatcher();
  }


}

bool WireWriteByte(uint8_t val)
{
    Wire.beginTransmission(LINE_FOLLOWER_I2C_ADDR); /* 选择地址开始传输 */
    Wire.write(val);  //发送数据
    if( Wire.endTransmission() != 0 ) {
        // Serial.println("false");  /* 发送失败 */
        return false;
    }
    // Serial.println("true");   /* 发送成功 */
    return true;
}
/* 按字节读数据 */
bool WireReadDataByte(uint8_t reg, uint8_t &val){
    if (!WireWriteByte(reg)) {   /* 给传感器发送读/写信号 */
        return false;
    }   
    Wire.requestFrom(LINE_FOLLOWER_I2C_ADDR, 1);/* 接收到传感器的应答信号 */
    while (Wire.available()) { /* 开始读取数据 */
        val = Wire.read();
    }   
    return true;
}

/* 电机初始化函数 */
void Motor_Init(void){
  for(uint8_t i = 0; i < 4; i++){
    pinMode(motordirectionPin[i], OUTPUT);
  }
  Velocity_Controller( 0, 0, 0, 0);
}

/**
 * @brief 速度控制函数
 * @param angle   用于控制小车的运动方向，小车以车头为0度方向，逆时针为正方向。
 *                取值为0~359
 * @param velocity   用于控制小车速度，取值为0~100。
 * @param rot     用于控制小车的自转速度，取值为-100~100，若大于0小车有一个逆
 *                 时针的自转速度，若小于0则有一个顺时针的自转速度。
 * @param drift   用于决定小车是否开启漂移功能，取值为0或1，若为0则开启，反之关闭。
 * @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;///< 速度因子
  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与轮子转向设置函数
 * @param Motor_x   作为PWM与电机转向的控制数值。根据麦克纳姆轮的运动学分析求得。
 * @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
  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]); 
  }
}

/* 获取传感器数据 */
void Sensor_Receive(void){
  WireReadDataByte(1,data);
  rec_data[0] = data & 0x01;
  rec_data[1] = (data>>1) & 0x01;
  rec_data[2] = (data>>2) & 0x01;
  rec_data[3] = (data>>3) & 0x01;
}

void Tracking_Line_Task(void){
  Rgb_Show(255,0,0);   
  if(rec_data[1] == 1 && rec_data[2] == 1){
    Velocity_Controller(0, 80, 0, 0);
  }
 if(rec_data[1] == 1 && rec_data[2] == 0){
    Velocity_Controller(0, 80, 65, 0);
  }
 if(rec_data[1] == 0 && rec_data[2] == 1){
    Velocity_Controller(0, 80, -65, 0);
  }
 while(rec_data[1] == 0 && rec_data[2] == 0){
    Sensor_Receive();
    Velocity_Controller(180, 80, 0, 0);
  }
  if(dis <= 80){
    modestate = AVOID;
  }

}

 /**
 * @brief 滤波
 * @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();     //读取超声波测值
  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);
}

/**
 * @brief 超声波距离数据获取
 * @param None
 * @arg None
 * @retval distance
 * @note None
 */
uint16_t ultrasonic_distance(){
  uint8_t s;
  uint16_t distance = Filter();         // 获得滤波器输出值
  if (distance > 0 && distance <= 80){
      ultrasound.Breathing(1, 0, 0, 1, 0, 0);       //呼吸灯模式，周期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); //红色渐变
   }
   
   else if (distance > 180 && distance <= 320){
      s = map(distance,180,320,0,255);
      ultrasound.Color(0, 0, s, 0, 0, s);            //蓝色渐变
   }
   
   else if (distance > 320 && distance <= 500){
      s = map(distance,320,500,0,255);
      ultrasound.Color(0, s, 255-s, 0, s, 255-s);            //绿色渐变
   }
  else if (distance > 500){
      ultrasound.Color(0, 255, 0, 0, 255, 0);        //绿色
   }
  return distance;  
}
/* 行人检测任务 */
void Avoid_Task(void){
  Velocity_Controller(0, 0, 0, 0);
  Rgb_Show(0,255,0);
  tone(buzzerPin, 1000);
  if(dis > 120) {
    modestate = TRACKING;
    noTone(buzzerPin);
  }
}
/* 任务调度 */
void Task_Dispatcher(){
	switch(modestate){
		case TRACKING:
			Tracking_Line_Task();
			break;
		case AVOID:
			Avoid_Task();
			break;
	}
}

 /**
 * @brief 设置RGB灯的颜色
 * @param rValue;gValue;bValue;
 * @arg 三个入口参数取值分别为:0~255;
 * @retval None
 * @note (255,0,0)绿色 (0,255,0)红色 (0,0,255)蓝色 (255,255,255)白色
 */
void Rgb_Show(uint8_t rValue,uint8_t gValue,uint8_t bValue) {
  rgbs[0].r = rValue;
  rgbs[0].g = gValue;
  rgbs[0].b = bValue;
  FastLED.show();
}