机器人及人工智能大赛 探索者全地形小车项目

机器人及人工智能大赛

站长参加的是探索者全地形小车项目，基本设计源于前几届学长的小车。

整体是一个六轮驱动的智能小车，同时需要有良好的力线使得不循迹也可以走较直的路线。车头部分的四轮为一体式结构，前轮需良好的攀爬能力——履带金属轮，前轮套上履带履片，扩大轮子直径，增强爬坡和越障能力的同时提高了移动速度。
有需要可以点击右边联络我捏～

整体实现代码

/*灰度传感器说明：左：中间值为750，小黑大白
                 右：中间值为750，小黑大白*/

#define ServoPin 12        //舵机引脚
#define s0 4               //定义颜色传感器引脚
#define s1 11              //定义颜色传感器引脚
#define s2 3               //定义颜色传感器引脚
#define s3 7               //定义颜色传感器引脚
#define outPin 8           //定义颜色传感器输出脚
#define PIN_x 2
#define PIN_y A3

int colorflag=0;           //颜色传感器返回值：红1绿2蓝3
int leftmotor1=5;          //左正转
int leftmotor2=6;         //左反转
int rightmotor1=9;         //右正转
int rightmotor2=10;         //右反转        `
int sensor1=14;             //左传感器
int sensor2=16;             //右传感器
int sensor3=17;             //加减速传感器
int leftread=0;            //左传感器读出数值
int rightread=0;           //右传感器读出数值
int middleleft=800;        //左传感器中间值
int middleright=800;       //右传感器中间值
//颜色传感器参数：
int pulseWidth;  
//读取参数
double r_ratio = 3.45, g_ratio = 2.62, b_ratio = 2.98;
int card=0;                //色卡值:红1绿2蓝3
int balloon=0;             //气球值：红1绿2蓝3


int red=0,green=0,blue=0;  //执行气球打击的相应标志位

void setup() {
  pinMode(leftmotor1,OUTPUT);
  pinMode(leftmotor2,OUTPUT);
  pinMode(rightmotor1,OUTPUT);
  pinMode(rightmotor2,OUTPUT);
  pinMode(ServoPin,OUTPUT);
  pinMode(sensor1,INPUT);
  pinMode(sensor2,INPUT);
  pinMode(sensor3,INPUT);
  xy_Init();
  Serial.begin(9600);

  digitalWrite(ServoPin, LOW);
}

/***************************************主循环函数****************************************************/
void loop() {
  walk_to_cross();
  
  
  //串口实时显示左右传感器灰度值
  Serial.print(leftread);
  Serial.print("\t");
  Serial.print(rightread);
  Serial.print("\n");

  colorflag=color_analyse();
  
}
/******************************************************************************************************/

void walk_to_cross()
{
  //读出灰度值
  leftread=Good_read(sensor1,51);
  rightread=Good_read(sensor2,51);
  
  if(leftread<=middleleft && rightread<=middleright)//全黑：爬坡以及爬台阶
  {
    delay(1);
    if(leftread<=middleleft && rightread<=middleright)//全黑：爬坡以及爬台阶
    {
      STOP();
      colorflag=color_analyse();
      if(colorflag)
      {
        if(card==0)//仍未读取色卡颜色值
        {
          deal_card();//色卡处理函数
        }
        
        //若已经读取色卡值则进入气球颜色读取函数
        if(card)
        {
          delay(1);
          if(card)//确认色卡值已读
          servo();
        }
      }
      else 
      {
        RUN();
        delay(2000);
      }
    }
  }
  else if(leftread>middleleft && rightread>middleright)//全白：普通直行
  {
    delay(1);
    if(leftread>middleleft && rightread>middleright)//全白：普通直行
    {
      forward();
    }
  }
  else if(leftread>middleleft && rightread<=middleright)//左转
  {
    delay(1);
    if(leftread>middleleft && rightread<=middleright)//左转
    {
      turn_left();
    }
  }
  else if(leftread<=middleleft && rightread>middleright)//右转
  {
    delay(1);
    if(leftread<=middleleft && rightread>middleright)//右转
    {
      turn_right();
    }
  }
  else forward();
}

//色卡处理函数
void deal_card()
{
  if(colorflag)//读取到色卡颜色
    {
      if(colorflag=='R')card=1;
      if(colorflag=='G')card=2;
      if(colorflag=='B')card=3;
      colorflag=0;//复位，以便下一次检测颜色
    }
}

//舵机控制击打气球
void servo()
{
  if(colorflag=='R')balloon=1;
  else if(colorflag=='G')balloon=2;
  else if(colorflag=='B')balloon=3;
  if(balloon==card)
  {
    STOP();
    ServoControl(180);
    delay(1500);
  }
}

/****滤波算法*************/
double  StandardDeviation(int a[], int cnt)
{
  double sum = 0;
  double ave = average(a, cnt);
  for (int i = 0; i < cnt; i++)
    sum += (1.0 * a[i] - ave) * (a[i] - ave);
  sum /= cnt;
  return sqrt(sum);
}
double  average(int a[], int cnt)
{
  double sum = 0;
  for (int i = 0; i < cnt; i++)     sum += 1.0 * a[i];
  sum /= cnt;
  return sum;
}
int judge_point(int a[], int cnt)
{
  double ave = average(a, cnt);
  double stand = StandardDeviation(a, cnt);
  int cntt = cnt;
  for (int i = 0; i < cnt; i++)
  {
    if (!(a[i] >= ave - 2 * stand && a[i] <= ave + 2 * stand))
    {
      cntt--;
      a[i] = 0;
    }
  }
  return average(a, cntt);
}
int Good_read(int PIN, int cnt)
{
  int a[cnt];
  for (int i = 0; i < cnt; i++) a[i] = analogRead (PIN);
  return judge_point(a, cnt);
}
float min1(float a, float b)
{
  return a <= b ? a : b;
}

//获取颜色标志位
void color()
{
  if(color_analyse()=='R')colorflag=1;//红
  if(color_analyse()=='G')colorflag=2;//绿
  if(color_analyse()=='B')colorflag=3;//蓝
}


//颜色识别
char color_analyse()
{
  int cnt = 50;
  int rColor1[cnt] ; //设置读取的red数值
  int gColor1[cnt] ; //设置读取的green数值
  int bColor1[cnt] ; //设置读取的blue数值
  int rColor ;  //设置读取的red数值
  int gColor ; //设置读取的green数值
  int bColor ; //设置读取的blue数值
  double r_ratio = 3.45, g_ratio = 2.62, b_ratio = 2.98;
  /*3.45  2.62  2.98 */
  double r_sum = 0, g_sum = 0, b_sum = 0;

  for (int i = 0; i < cnt; i++)
  {
    read_color(&rColor1[i], &gColor1[i], &bColor1[i]);
  }
  rColor = judge_point(rColor1, cnt);
  gColor = judge_point(gColor1, cnt);
  bColor = judge_point(bColor1, cnt);
  Serial.print(rColor * r_ratio);
  Serial.print("  ");
  Serial.print(gColor * g_ratio);
  Serial.print("  ");
  Serial.print(bColor * b_ratio);
  Serial.print("  ");
  if ( rColor * r_ratio < gColor * g_ratio && rColor * r_ratio < bColor * b_ratio)
  { Serial.println("R");
    return 'R';
  }
  else if (gColor * g_ratio < rColor * r_ratio && gColor * g_ratio < bColor * b_ratio)
  { Serial.println("G");
    return 'G';

  } else if (bColor * b_ratio < rColor * r_ratio && bColor * b_ratio < gColor * g_ratio)
  { Serial.println("B");
    return 'B';
  }
}

//颜色读取
void read_color(int *rColor, int *gColor, int  *bColor)
{
  int cnt = 1;
  int tmp_r = 0, tmp_g = 0, tmp_b = 0;
  for (int i = 0; i < cnt; i++)
  {
    digitalWrite(13, 1);
    delay(3);
    //红色读取
    digitalWrite(s2, LOW);
    digitalWrite(s3, LOW);
    pulseWidth = pulseIn(outPin, LOW);
    tmp_r += pulseWidth;
    delay(3);

    //绿色读取
    digitalWrite(s2, HIGH);
    digitalWrite(s3, HIGH);
    pulseWidth = pulseIn(outPin, LOW);
    tmp_g += pulseWidth;
    delay(3);

    //蓝色读取
    digitalWrite(s2, LOW);
    digitalWrite(s3, HIGH);
    pulseWidth = pulseIn(outPin, LOW);
    // Serial.println(pulseWidth);
    tmp_b += pulseWidth;
    digitalWrite(13, 0);
    delay(3);
  }
  *rColor = tmp_r / cnt;
  *gColor = tmp_g / cnt;
  *bColor = tmp_b / cnt;
}

//普通直行函数
void forward(void){
  analogWrite(leftmotor1,210);
  analogWrite(leftmotor2,0);
  analogWrite(rightmotor1,210);
  analogWrite(rightmotor2,0);
}

//右转函数
void turn_right(void){
  analogWrite(leftmotor1,0);
  analogWrite(leftmotor2,200);
  analogWrite(rightmotor1,255);
  analogWrite(rightmotor2,0);
  }
  
//左转函数
void turn_left(void){
  analogWrite(leftmotor1,255);
  analogWrite(leftmotor2,0);
  analogWrite(rightmotor1,0);
  analogWrite(rightmotor2,200);
  }

//冲刺函数
void RUN(void){
  analogWrite(leftmotor1,255);
  analogWrite(leftmotor2,0);
  analogWrite(rightmotor1,255);
  analogWrite(rightmotor2,0);
  
}

//停车函数
void STOP(void){
  analogWrite(leftmotor1,0);
  analogWrite(leftmotor2,0);
  analogWrite(rightmotor1,0);
  analogWrite(rightmotor2,0);
}

//加速度读取
int xy_Read()   //长时间大于200说明上不去楼梯
{
  int cnt = 50;
  int a[cnt];
  for (int i = 0; i < cnt; i++) a[i] = Good_read(PIN_y, cnt);
  return StandardDeviation(a, cnt);
}


//加速度传感器初始化
void xy_Init()
{
  pinMode(PIN_x, INPUT);
  pinMode(PIN_y, INPUT);
}

//舵机控制函数
void ServoControl(int servoAngle)
{
  double thisAngle = map(servoAngle, 0, 270, 500, 2500);//等比例角度值范围转换高电平持续时间范围
  unsigned char i = 50;//50Hz 每秒的周期次数(周期/秒) 即1S 50 个周期 每个周期20ms
  while (i--)
  {
    digitalWrite(ServoPin, HIGH); 
    delayMicroseconds(thisAngle); //高电平时间
    digitalWrite(ServoPin, LOW); 
    delayMicroseconds(20000 - thisAngle);//每个周期20ms减去高电平持续时间
  }
}


long map(long x,long in_min,long in_max,long out_min,long out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

滤波算法

/****滤波算法*************/
double  StandardDeviation(int a[], int cnt)
{
  double sum = 0;
  double ave = average(a, cnt);
  for (int i = 0; i < cnt; i++)
    sum += (1.0 * a[i] - ave) * (a[i] - ave);
  sum /= cnt;
  return sqrt(sum);
}
double  average(int a[], int cnt)
{
  double sum = 0;
  for (int i = 0; i < cnt; i++)     sum += 1.0 * a[i];
  sum /= cnt;
  return sum;
}
int judge_point(int a[], int cnt)
{
  double ave = average(a, cnt);
  double stand = StandardDeviation(a, cnt);
  int cntt = cnt;
  for (int i = 0; i < cnt; i++)
  {
    if (!(a[i] >= ave - 2 * stand && a[i] <= ave + 2 * stand))
    {
      cntt--;
      a[i] = 0;
    }
  }
  return average(a, cntt);
}
int Good_read(int PIN, int cnt)
{
  int a[cnt];
  for (int i = 0; i < cnt; i++) a[i] = analogRead (PIN);
  return judge_point(a, cnt);
}
float min1(float a, float b)
{
  return a <= b ? a : b;
}

对应讲解

滤波算法的核心函数是 StandardDeviation 和 average。
average 函数用于计算给定数组 a 中元素的平均值。它首先对数组中的所有元素求和，然后将总和除以数组的长度，得到平均值。
StandardDeviation 函数用于计算给定数组 a 中元素的标准差。它首先调用 average 函数计算数组的平均值 ave。然后通过遍历数组，计算每个元素与平均值的差的平方，并累加到 sum 变量中。最后，将 sum 除以数组的长度，得到方差。然后使用 sqrt 函数对方差进行平方根运算，得到标准差，即传感器读数的波动程度。
judge_point 函数使用了滤波算法对传感器读数进行处理，并剔除超出平均值加减两倍标准差范围之外的数据。它首先调用 average 和 StandardDeviation 函数计算数组的平均值和标准差。然后遍历数组，判断每个元素是否在平均值加减两倍标准差范围之内。如果不在范围内，将该元素置为0，并将有效元素的数量减1。最后，计算剔除异常数据后的数组的平均值，并返回该平均值。
在 Good_read 函数中，首先创建了一个数组 a，用于存储传感器的读数。然后使用循环读取传感器的值，并将其存储到数组中。最后调用 judge_point 函数对读数进行滤波处理，并返回滤波后的平均值。
这样，在主程序的 run 函数中，就可以使用滤波后的传感器读数来判断小车的行驶方向，提高对地形变化的适应能力。
通过应用滤波算法，可以有效地减少传感器读数中的噪声和波动，提高系统的稳定性和可靠性，使小车能够更准确地感知和应对各种地形条件。