Lesson 16: Final Competition and Showcase

/**
 * @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();
}