arduino黑线循迹小车程序_基于arduino的循迹小车(含有PID算法)

循迹小车一般分为两方面:一方面是简单的闭环赛道只有直道和弯道,另一方面是毕设类型的包括一些元素:90度弯道、十字道路、S形弯道等。

本篇博客试根据下图来进行书写的,如果大家有什么新的元素,也可以在下方评论,我进行更新。

4d141245ca4814a5416bd46a9f71809b.png

作者:sumjess

注意本篇博客循迹模块使用了5个

一、简单的闭环赛道

随意画了一个

15e5ae6a33029a39cbf360fe76e87bc9.png

(1)逻辑部分:

   ecb65c85bf2e0ef44af201ba8d04d5ff.png

所以程序的写法也很简单,就是检测到哪种情况对应着哪种反应。这一过程可以用switch也可以用if来实现这一过程。下文用if来演示。

(2)各程序片段

总的循环:

void loop(){     read_sensor_values();  //获取5个循迹模块的数值情况     calc_pid();            //pid计算出转向的pwm值     motor_control();       //电机转动}

第一部分:检测部分程序片段

void read_sensor_values(){  sensor[0] = digitalRead(leftA_track_PIN);  sensor[1] = digitalRead(leftB_track_PIN);  sensor[2] = digitalRead(middle_track_PIN);  sensor[3] = digitalRead(righA_track_PIN);  sensor[4] = digitalRead(righB_track_PIN);    if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1)) {      error = 2;//          0 0 0 0 1    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 0)) {      error = 1;//          0 0 0 1 0    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {      error = 0;//          0 0 1 0 0    } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {      error = -1;//         0 1 0 0 0    } else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {      error = -2;//         1 0 0 0 0    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {      if (error == -2) {//  0 0 0 0 0        error = -3;      }else{        error = 3;      }    }  }

解释:

<1>  将读取到的五个循迹模块的数据存入数组sensor:

  sensor[0] = digitalRead(leftA_track_PIN);      左边第一个循迹模块

  sensor[1] = digitalRead(leftB_track_PIN);      左边第二个循迹模块

  sensor[2] = digitalRead(middle_track_PIN);   中间的循迹模块

  sensor[3] = digitalRead(righA_track_PIN);     右边第二个循迹模块

  sensor[4] = digitalRead(righB_track_PIN);     右边第一个循迹模块

<2>  检测到哪种情况对应着哪种反应:

if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1)) {

      error = 2;//          0 0 0 0 1

    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 0)) {

      error = 1;//          0 0 0 1 0

    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {

      error = 0;//          0 0 1 0 0

    } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {

      error = -1;//         0 1 0 0 0

    } else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {

      error = -2;//         1 0 0 0 0

    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {

      if (error == -2) {//  0 0 0 0 0

        error = -3;

      }else{

        error = 3;

      }

    }

前面几个就不解释了完全按照前面excel写的,最后一个是所有传感器均没有检测到黑线的情况,下面具体解释一下:

else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {

 检测到为0000的情况

      if (error == -2) {// //如果上一次 error == -2

意味着上一次是1000,也就是意味着这次可能车在左面第一个传感器和第二个传感器之间或者是在左边第一个传感器的左边,无论是上面的哪种情况,车都需要大左转

        error = -3;

      }else{

否则就是相反的情况,都需要大右转

        error = 3;

      }

    }

第二部分:计算PID(计算转向大小)

void calc_pid(){  P = error;  I = I + error;  D = error - previous_error;   PID_value = (Kp * P) + (Ki * I) + (Kd * D);   previous_error = error;}

利用上一部分得到的error计算车的偏离情况,车偏离赛道的情况从而来调整下一次给出的PWM进而快速转正车身。

这一部分的kp、ki、kd需要不断地调试,从而得出最佳解。

第三部分:电机转向

//速度设定范围(-255,255)void motorsWrite(int speedL, int speedR){  if (speedR > 0) {    analogWrite(leftA_PIN, speedR);    analogWrite(leftB_PIN, 0);  } else {    analogWrite(leftA_PIN, 0);    analogWrite(leftB_PIN, -speedR);  }   if (speedL > 0) {    analogWrite(righA_PIN, speedL);    analogWrite(righB_PIN, 0);  } else {    analogWrite(righA_PIN, 0);    analogWrite(righB_PIN, -speedL);  }}//速度设定范围(-100,100)void motorsWritePct(int speedLpct, int speedRpct) {  //speedLpct, speedRpct ranges from -100 to 100  motorsWrite(speedLpct * 2.55, speedRpct * 2.55);}void motor_control(){  int left_motor_speed = initial_motor_speed - PID_value;  int right_motor_speed = initial_motor_speed + PID_value;    if(left_motor_speed < -255){    left_motor_speed = -255;  }  if(left_motor_speed > 255){    left_motor_speed = 255;  }  motorsWrite(left_motor_speed,right_motor_speed);} 

第一个函数是在检查更改正负值,来保证PWM都是正的,即轮子不会倒转。

第二个函数是在利用一次函数,将输入范围变成0-100

第三个函数是在控制范围,ardunio的PWM范围是在正负255,此函数是在做一个限制·

二、在上面的基础上添加元素

c8648b77fc2fb938a476302c46def668.png

(1)逻辑部分:

3acba346b3e34e3622d274e195356e9e.png

(2)各程序片段

除了read_sensor_values() 各程序片段与之前都一样,read_sensor_values() 只是添加了一部分,情况如下:

void read_sensor_values(){  sensor[0] = digitalRead(leftA_track_PIN);  sensor[1] = digitalRead(leftB_track_PIN);  sensor[2] = digitalRead(middle_track_PIN);  sensor[3] = digitalRead(righA_track_PIN);  sensor[4] = digitalRead(righB_track_PIN);        if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1)) {      decide = 1;//十字路口 1 1 1 1 1   直行    } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0)) {      decide = 1;//十字路口 0 1 1 1 0   直行    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1)) {      decide = 2;//90度路口 0 0 1 1 1    右转90度    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0)) {      decide = 2;//90度路口 0 0 1 1 0    右转90度     } else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {      decide = 3;//90度路口 1 1 1 0 0    左转90度     } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {      decide = 3;//90度路口 0 1 1 0 0    左转90度     } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {      decide = 3;//向上锐角 0 1 1 0 0    向上锐角     } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 1)) {      error = 2;//          0 0 0 0 1    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 0)) {      error = 1;//          0 0 0 1 0    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 0)) {      error = 0;//          0 0 1 0 0    } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {      error = -1;//         0 1 0 0 0    } else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {      error = -2;//         1 0 0 0 0    } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0) && (sensor[3] == 0) && (sensor[4] == 0)) {      if (error == -2) {//  0 0 0 0 0        error = -3;      }else{        error = 3;      }    } }

各个条件都是按照前面excel写的

附全套程序:

循迹小车含PID程序:

链接:

https://pan.baidu.com/s/10tJzqiAO0Ph86aIfmUzrHg 

提取码:x6vc 

循迹小车不含PID程序:

链接:

https://pan.baidu.com/s/1EckZUv66bMJdf7SecpYqcw 

提取码:au9d 

已标记关键词 清除标记
表情包
插入表情
评论将由博主筛选后显示,对所有人可见 | 还能输入1000个字符
相关推荐
©️2020 CSDN 皮肤主题: 深蓝海洋 设计师:CSDN官方博客 返回首页