循迹的光电搬运小车含PID（硬件+源代码+3D文件）

版权声明：本文为博主原创文章，转载请附上原文出处连接与本声明。
本文链接：https://blog.csdn.net/luoqisimieji/article/details/103107314.
—————————————————————————————————————————————

  第一次写博客，可能显得比较杂乱。

  本人为非985/211的大二男一枚，所以技术水平你懂的，程序中bug很多，但是代码还是能拿出去比赛的（哭.jpg），所以希望各位大佬能指出其中的bug，也欢迎大家一起学习改进。

  文章着重强调了循迹算法以及编码器的使用，其他的算法如舵机和比赛任务实现算法等可在文章最后的链接里全部下载。
—————————————————————————————————————————————

一. 设计思路

1. 比赛规则

  小车是按照江苏省机器人大赛光电搬运的要求设计的。想了解这个比赛的朋友可以看一下官网
　　江苏省大学生机器人大赛官网链接

  地图是这样

 
 
大致规则是：
　　一共有黑白红绿蓝五种圆形物块，比赛前抽签决定摆放顺序，按抽签结果依次放在F区和G区的A、B、C、D、E五个区域，同时在地图的A、C、E三个区域（不是F区和G区的ACE）放上对应于F区（或G区）A、C、E的颜色的物块，B、D不放物块。
　　小车从地图底部出发，先按颜色放完地图上Ａ、Ｃ、Ｅ的物块，再到Ｆ区和Ｇ区取物块（Ｆ、Ｇ区的物块没有放置顺序要求）放到对应颜色区域。也就是要实现物块的堆积存放。

  完整规则请参考：http://robotmatch.cn/contents/938/6081.html

2. 设计思路

  首先是底板，这玩意儿是要靠自己用CAD去画的，为了画个破底板还专门区bilibili大学学了一下。
  搬运最需要的是精准度，要考虑到：如果因为舵机的动作不当或者是地图不平整导致物块放歪掉了怎么办。参考了别人的解决方案再加以改进后，我决定用三个连续的弧形凹槽。这样的话在小车前进过程中，物块如果出现了一点点的偏移，可以通过圆弧实现自动归位。

二. 硬件需求

  首先推荐一下主板（只是觉得好用，没收任何广告费），可能大家都会用arduino之类的主板。但是考虑到arduino价格实在太贵，并且计算能力不足，引脚不够，我买的是一个叫零知的主板，和arduino操作几乎一模一样，是可以兼容aruino的。而且人家是开源的，就冲这一点也应该支持一下不是？（虽然我买这个主要原因还是便宜，因为在第一次组装机器人的过程中难免会烧板子，烧一块arduino就。。。。。）
  附零知官网：http://www.lingzhilab.com/

  测色需要颜色传感器；循迹需要循迹模块；判断到线停我用了激光传感器；实现物块堆积需要套筒；运动需要电机；搬运动作的实现需要用舵机；为了行进的准确度我选择用自带编码器的电机。这些是核心元件，还有一些杜邦线，开关，电池，变压器，铜柱，螺丝螺帽等等。整套设备不太便宜，几千块。因为这是比赛，学校是报销费用的，因此这个机器人是不适合DIY的。不过对于简单的循迹小车，循迹算法可以参考。

  以下是全部原件列表：

名称	型号	用途
零知主控板	1个零知开源零知增强板M4(STM32F407VET6)	主板
循迹模块	2个未来机器人八路灰度传感器	循迹
激光传感器	4个未来机器人漫反射激光传感器	数线
舵机	9个9gEMAX ES08A模拟舵机、1个MG99620KG舵机	搬物块
电机	2个AB相编码器直流减速电机	运动
电机驱动板	AQMH2407ND	控制电机
万向轮	随便一个万向轮	从动轮
颜色传感器	GY-31 TCS230 TCS3200	测颜色
滑杆	18cm滑杆	固定套筒，实现物块的抬高与堆放
套筒	自己画CAD设计，没得卖	堆物块
变压器	2个12v转5v的直流电池变压器	将电池的12.6V转为5V与6V

主要器件如下：

三. 算法设计

1.循迹部分

  循迹需要靠循迹模块的返回值，我用的循迹模块有八个灯，其实完全用不到这么多，两个灯足以。因为搬运物块不需要太大的速度，精准最重要，因此不会出现特别大的出线情况。因此，中间的4、5号灯足够完成任务。这个循迹传感器是模拟量的，需要接在主板上模拟量接口A0~A8中的任意四个（因为前进后退各需要两个灯）。它的返回值是0-4095，经过测量，发现在黑线上返回值不会超过1000（大概在），在白色区域稳定大于3000。

（1）读取灰度传感器数值

val4、val5为前进方向4、5号灯的读数，6、7号灯为后退方向4、5号灯的读数

int error = 0 ;
void getABCD()
{ //获取4、5、6、7号灯值

  val4 = 0;
  val5 = 0;
  val6 = 0;
  val7 = 0;
  int i = 0;
  ///滤波
  for (i = 0; i < 4; i++) {
    val4 += analogRead(Qvalue4);
    val5 += analogRead(Qvalue5);
    val6 += analogRead(Hvalue4);
    val7 += analogRead(Hvalue5);
  }
  val4 = val4 / 4;
  val5 = val5 / 4;
  val6 = val6 / 4;
  val7 = val7 / 4;
  
  if (flag == 1) {//前进flag设为1
    if (val4 < 800 && val5 < 600)//在黑线
      error = 0;
    else if (val4 < 800 && val5 > 600)//偏右
      error = 3;
    else if (val4 > 800&& val5 < 600)//偏左
      error = 3;
    else
      error = 0;
  }
  else {//后退
    if (val6 < 600 && val7 <1000)//在黑线
      error = 0;
    else if (val6 < 600 && val7 > 1000)//偏右
      error = 3;
    else if (val6 > 600 && val7 < 1000)//偏左
      error = 3;
    else
      error = 0;
  }
}

读取四次是为了减少偶然误差。
在循迹之前调用这个算法，它返回的误差error是实现小车左右轮速改变的关键。

（2）前进后退算法

void goStraight(int speeda, int speedb) {//B为右轮，A为左轮
  digitalWrite(MotorApin1, LOW);
  digitalWrite(MotorApin2, HIGH);
  analogWrite(MotorApwm, speeda);

  digitalWrite(MotorBpin1, HIGH);
  digitalWrite(MotorBpin2, LOW);
  analogWrite(MotorBpwm, speedb);
}

void retreat(int speeda, int speedb) {
  digitalWrite(MotorApin1, HIGH);
  digitalWrite(MotorApin2, LOW);
  analogWrite(MotorApwm, speeda);

  digitalWrite(MotorBpin1, LOW);
  digitalWrite(MotorBpin2, HIGH);
  analogWrite(MotorBpwm, speedb);
}

  这个算法没什么特别的，电机驱动板上一个轮子由三根线控制，在宏定义里分别对应MotorApin1，MotorApin2，MotorApwm。pin1,pin2是传输数字量，一高一低即传输前进信号，一低一高代表后退信号，pwm接pwm口，类似于舵机，范围为0-255，pwm值越大代表速度越大。

（3）循迹算法（敲黑板！重点来了！！）

经过从暑假到现在几个月，一共有三种算法，现在都分享给大家。

算法一：

void getABCD()
{ //获取4、5、6、7号灯值

  val4 = 0;
  val5 = 0;
  val6 = 0;
  val7 = 0;
  int i;
  for (i = 0; i < 4; i++) {
    val4 += analogRead(QI4);
    val5 += analogRead(QI5);
    val7 += analogRead(HI4);
    val6 += analogRead(HI5);
  }
  val4 = val4 / 4;
  val5 = val5 / 4;
  val6 = val6 / 4;
  val7 = val7 / 4;
}

int change = 40 ;

void yanxianzou(int speeda, int speedb) {//沿线走
  int average = 1500;//(QvalueMax + QvalueMin )/2
  getABCD();
  if (val4 < average && val5 < average ) { //全黑
    goStraight(speeda,speedb);
  }
  if (val4 < average && val5 > average) { //偏右
    goStraight(speeda + change ,speedb - change );
  }//右轮加速，左轮减速
  if (val4 > average && val5 < average) { //偏左
   goStraight(speeda - change ,speedb + change );
  }//右轮减速，左轮加速
  if (val4 > average && val5 > average) { //全白
    goStraight(speeda,speedb);
  }//右轮保持，左轮保持

  （请注意，此处的 getABCD() 函数不是上面的，上面是经过改进的代码，算法一算法二都是之前用的代码，现在不用了，之所以不用不是因为不能用，而是因为希望大家共同看看我的代码，哪里是好的可以保留，哪里有问题需要删除嘿嘿嘿。）
  关于代码用汉语拼音命名这件事大家就不要吐槽了哈。（尴尬）
  这个算法是我从上一届学长那里继承的算法，比较简单，它的核心就是：调用goStraight函数之前判断小小车在不在黑线上，如果偏移则按相应的方向调整一个已经宏定义过的固定的pwm值change。
  这种算法的缺点，就是虽然走直线很稳，但没法走圆弧。顺便说一句，走圆弧在本文说的江苏省比赛意义不大，看一下地图就会发现问题。但是还有个比赛是全国的大学生机器人比赛，那张地图走圆弧可以省很多的时间。下面就是一个我在国赛时写的可以走圆弧的代码。

算法二：

void getABCD()
{ //获取4、5、6、7号灯值

  val4 = 0;
  val5 = 0;
  val6 = 0;
  val7 = 0;
  int i;
  for (i = 0; i < 4; i++) {
    val4 += analogRead(QI4);
    val5 += analogRead(QI5);
    val7 += analogRead(HI4);
    val6 += analogRead(HI5);
  }
  val4 = val4 / 4;
  val5 = val5 / 4;
  val6 = val6 / 4;
  val7 = val7 / 4;
}
void Sbigcircle(int speeda , int speedb )//顺时针走大圆
{
  getABCD();
  int average = (QvalueMax + QvalueMin)/2 ;//1850
  P_error1 = val4 - average ;
    if(P_error1 < 0) 
    P_error1 = 0 ;
  D_error1 = P_error1 - D_error1 ;
  I_error1 += P_error1 ;
  P_error2 = val5 - average ;
    if(P_error2 < 0) 
    P_error2 = 0 ;
  D_error2 = P_error2 - D_error2 ;
  I_error2 += P_error2 ;

  if(val4 < average + 1000 && val5 < average )
  {
    I_error1 = 0 ;
    I_error2 = 0 ;
  }
  double tem = 0.8 * Kp1;
  
  if(val4 < 600 && val5 < 600)
  goStraight(speeda - 10 ,speedb + 25);
  else if(val4 < 1000 && val5 > 2000)//偏右   1.3倍Kp稳定
  goStraight(speeda + Kp1 *P_error1 + Ki1 * I_error1+ Kd1 *D_error1   ,speedb - Kp1 *P_error2  - Ki1 * I_error2 - Kd1 *D_error2 );
  else if(val4 > 2000  && val5 < 1000 )//偏左
  goStraight(speeda - Kp1 *P_error1 - Ki1 * I_error1 - Kd1 *D_error1 - 10 ,speedb +  Kp1 *P_error2 + Ki1 * I_error2 + Kd1 *D_error2 + 25);
  else 
  goStraight(speeda - 10 ,speedb + 25 );//25
}

  （请注意，此处的 getABCD() 函数同样不是上面的。）
  这里用到了PID算法。关于PID是什么我在这里不多讲解，因为篇幅太长了，给大家一个网站大家可以自己参考一下：
  循迹小车的PID原理（似乎只能用手机打开)¹

  看完上面这个，相信大家应该都懂了，因为我大一暑假初次接触PID时，就是靠这篇文章看懂了PID，感谢大佬的分享！

  在算法二里，我首先构建一个比例函数，以灰度传感器读取的数值减去阈值（P_error1 = val4 - average）为自变量，以输出的pwm改变值为因变量。这样构建了y = k*x 的函数，可以实现当读数越大（即灯越来越靠近白色区域，亦即偏移越来越大）时，pwm相应的改变值也就越大，从而实现了在圆弧上精准的循迹。
  但是在实际过程中，这个算法有个致命的缺陷，就是在从圆弧进入直线的过程中极度不稳定。虽然单独让它走直线也可以，但就是在从圆弧进入直线的过程中屡次出现问题。因此我放弃了用这个算法走直线，只让它走圆弧，走直线用的还是算法一。（毕竟这个算法都被他们用了几年了，从实际情况来看，机器人走路稳的一批。）
  

算法三：

/循迹
const float Kp1 = 13, Ki1 = 0.25, Kd1 = 1.5;                    //pid前进参数参数 
const float Kp2 = 11, Ki2 = 0.2, Kd2 = 4.5;                    //pid后退参数参数 
float error = 0, P = 0, I = 0, D = 0, PID_value = 0; 
float previous_error = 0, previous_I = 0;

void getABCD()
{ //获取4、5、6、7号灯值

  val4 = 0;
  val5 = 0;
  val6 = 0;
  val7 = 0;
  int i = 0;
  ///滤波
  for (i = 0; i < 4; i++) {
    val4 += analogRead(Qvalue4);
    val5 += analogRead(Qvalue5);
    val6 += analogRead(Hvalue4);
    val7 += analogRead(Hvalue5);
  }
  val4 = val4 / 4;
  val5 = val5 / 4;
  val6 = val6 / 4;
  val7 = val7 / 4;
  
  if (flag == 1) {//前进flag设为1
    if (val4 < 800 && val5 < 600)//在黑线
      error = 0;
    else if (val4 < 800 && val5 > 600)//偏右
      error = 3;
    else if (val4 > 800&& val5 < 600)//偏左
      error = 3;
    else
      error = 0;
  }
  else {//后退
    if (val6 < 600 && val7 <1000)//在黑线
      error = 0;
    else if (val6 < 600 && val7 > 1000)//偏右
      error = 3;
    else if (val6 > 600 && val7 < 1000)//偏左
      error = 3;
    else
      error = 0;
  }
}

void calc_pid() {
  P = error;
  I = I + error;
  D = error - previous_error;
  
  if(flag == 1)
  {
	  if (I > 30) I = 10;
	  PID_value = (Kp1 * P) + (Ki1 * I) + (Kd1* D);
  }
  else 
  {
      if (I > 25) I = 15;
	  PID_value = (Kp2 * P) + (Ki2 * I) + (Kd2* D);
  }
  previous_error = error;
}

void yanxianzou(int a, int b) {
  flag = 1;
  getABCD();
  calc_pid();
  if (val4 < 1000 && val5 < 900)//在黑线
    goStraight(a, b);
  else if (val4 < 1000 && val5 > 900)//偏右
    goStraight(a + PID_value, b - PID_value);
  else if (val4 > 1000&& val5 < 900)//偏左
    goStraight(a - PID_value, b + PID_value);
  else
    goStraight(a, b);
}

void yanxiantui(int a, int b) {
  flag = 0;
  getABCD();
  calc_pid();
  if (val6 < 800 && val7 <1400)//在黑线
    retreat(a, b);
  else if (val6 >  800  && val7 < 1400)//偏左
    retreat(a - PID_value, b +PID_value);
  else if (val6 <  800  && val7 > 1400)//偏右
    retreat(a+PID_value , b - PID_value);
  else
    retreat(a, b);
}

  这是目前我用的算法，我把PID的计算单独写了一个函数，看起来更简洁明了，同时可以用一个函数实现调用不同的Kp,Ki,Kd。在这里强调一下，一个速度对应一套PID比例参数，否则可能会不稳。而且可以发现，改进后的方法介于算法一和算法二之间，不像算法一那样简单机械，也不像算法二那样生搬硬套大佬的思路。因为在走直线的时候，Ki 对小车动作改变非常小，此时这个算法更像算法一；而在走圆弧的时候，因为有一个灯长时间在白色区域，随着时间的累计 Ki 的作用越来越大，走圆弧就特别稳定，此时更像算法二。
  有些大佬可能已经发现我这个PID算法的问题了，那就是：PID要求严格按照时间采样，你这个算法怎么没有体现时间呢，没有在固定的单位时间内采样你这个算法准吗。我的回答是：大佬你说对了，我这个确实没有考虑时间，因为当时还不会用定时器中断。用了定时器中断之后就没有这个问题了，这也是我下一步需要修改的地方，之后完成的话我还会更新，欢迎大家在评论区催我！但是呢，从实际效果来看，小车走得还是挺稳的，所以当时就没急着改。所以……别碰这代码，他是可用的！！！

  顺便说一句，零知的主板似乎不能用MsTimer2.h等这样的定时器函数库，原因我也不知道，但是我的老师给了我一种解决办法，代码如下：

void doSomethingHere(HardwareTimer*)
{ 
}
void MsTimer_to_do()
{
  TIM_TypeDef *Instance = TIM2;
  // Instantiate HardwareTimer object. Thanks to 'new' instanciation, HardwareTimer is not destructed when setup() function is finished.
  HardwareTimer *MyTim = new HardwareTimer(Instance);
  MyTim->setMode(2, TIMER_OUTPUT_COMPARE);  // In our case, channekFalling is configured but not really used. Nevertheless it would be possible to attach a callback to channel compare match.
  MyTim->setOverflow(10, HERTZ_FORMAT); // 10 Hz
  MyTim->attachInterrupt(doSomethingHere);
  MyTim->resume();
}
void setup(){
  MsTimer_to_do();
}

void loop()
{
  /* Nothing to do all is done by hardware. Even no interrupt required. */
}

  如果想要用定时器中断做什么，只要写在 doSomethingHere() 这个函数里就行，目前还在改进代码的过程中，我打算用这个函数来实现每隔一定时间读取一次灰度传感器与电机编码器的值。如果写完了我就会更新。

2.编码器

由于暑假的时候还没采用定时器中断写编码器，这里的编码器函数仅供参考：

void doMotorA()
{
  if (digitalRead(MotorAcountA) == HIGH)
  {
    if (digitalRead(MotorAcountB) == HIGH)
    {
      MotorAPos = MotorAPos + 1;
    }
    else
    {
      MotorAPos = MotorAPos - 1;
    }
  }
  else
  {
    if (digitalRead(MotorAcountB) == LOW)
    {
      MotorAPos = MotorAPos + 1;
    }
    else
    {
      MotorAPos = MotorAPos - 1;
    }
  }
}

void doMotorB()
{
  if (digitalRead(MotorBcountA) == HIGH)
  {
    if (digitalRead(MotorBcountB) == LOW)
    {
      MotorBPos = MotorBPos + 1;
    }
    else
    {
      MotorBPos = MotorBPos - 1;
    }
  }
  else
  {
    if (digitalRead(MotorBcountB) == HIGH)
    {
      MotorBPos = MotorBPos + 1;
    }
    else
    {
      MotorBPos = MotorBPos - 1;
    }
  }
}

void move_compute(int* speeda, int* speedb)
{
  now_time = old_time + 50;
  while (millis() < now_time);
  attachInterrupt(digitalPinToInterrupt(MotorAcountA),doMotorA,CHANGE);
  attachInterrupt(digitalPinToInterrupt(MotorBcountA),doMotorB,CHANGE);
  //detachInterrupt(digitalPinToInterrupt(MotorAcountA));
    //detachInterrupt(digitalPinToInterrupt(MotorBcountA));
  omgaA = abs((float)(MotorAPos - old_posA) * 20 / onecircle);//计算左轮转速
  omgaB = abs((float)(MotorBPos - old_posB) * 20 / onecircle);//计算右轮转速
//  Serial.print("omgaA=");
//  Serial.print(omgaA);
//  Serial.print("r\\s omgaB=");
//  Serial.print(omgaB);
//  Serial.print("r\\s");
//  Serial.println("");
  float t1 = (float)(*(speeda));
  float t2 = (float)(*(speedb));
  if(omgaA>omgaB && (omgaA - omgaB)>0.1) 
  {
    *(speedb) += 3; //= (float)(t2 - 100*(omgaA - omgaB));
    *(speeda) -= 2; //= (float)(t1 + 100*(omgaA - omgaB));
  }
  else if(omgaA< omgaB && (omgaB - omgaA)>0.1)
  {
    *(speedb) -= 2;// (float)(t2 + 80*(omgaB - omgaA));
    *(speeda) += 3;// (float)(t1 - 80*(omgaB - omgaA));
  }
  if (*(speedb) > t2 * 1.2 || *(speedb) < t2 * 0.1)  *(speedb) = t2;
  if (*(speeda) > t1 * 1.2 || *(speeda) < t1 * 0.1) *(speeda) = t1;//防止轮子因意外速度太大
  //实际过程中快速拨动其中一个轮子，另一个轮子速度增大且两轮均无法变为原速
  old_time = millis();
  old_posA = MotorAPos;
  old_posB = MotorBPos;
//  attachInterrupt(digitalPinToInterrupt(MotorAcountA),doMotorA,CHANGE);
//  attachInterrupt(digitalPinToInterrupt(MotorBcountA),doMotorB,CHANGE);
}

这不算一个好的编码器代码，因为

  now_time = old_time + 50;
  while (millis() < now_time);

   这一行代码有一个50ms的循环，在这个50ms里面，其实机器人是没有做什么事的，就是等待跳出这个循环。在定时器终端使用后，我将对编码器代码进行改进。
   写这段代码的目的是使左右轮转速相同，因为在转弯的时候，左右轮速的不同将带来一些不稳定的因素。
  下面是转弯45度的一个算法：

void zuozhuan1(int speeda, int speedb) {//B为右轮，A为左轮      不用调用
 digitalWrite(MotorApin1, LOW);
 digitalWrite(MotorApin2, HIGH);
 analogWrite(MotorApwm, speeda);

 digitalWrite(MotorBpin1, LOW);
 digitalWrite(MotorBpin2, HIGH);
 analogWrite(MotorBpwm, speedb);
}

void zuozhuan(int speeda, int speedb)
{
 move_compute(&speeda, &speedb);
 zuozhuan1(speeda, speedb);
}
void turnLeftArrive(int sa,int sb){//A面左转左侧激光灯到达黑线（没有封装停）
   unsigned long times;
   int flag=0;
   int lj=0;
   while(flag<1){
   	zuozhuan(sa,sb);
   	lj=digitalRead(Llaser1);
   	times=millis();
   	while(lj && flag<1){
   		zuozhuan(sa,sb);
   		if(millis()-times >10 ){
   			flag++;
   		}
   		lj = digitalRead(Llaser1);
   	}
   }
}
void turnRightArrive(int sa,int sb){//A面向右转右激光灯到达黑线（没有封装停）
 unsigned long times;
 int flag = 0;
 int rj=0;
 while (flag < 1){
   youzhuan(sa,sb);
   rj= digitalRead(Llaser1);
   times = millis();
   while (rj && flag < 1){
     youzhuan(sa,sb);
     //times++;
     if (millis() - times >10 ){//35
       flag++;
     }
     rj = digitalRead(Llaser1);
   }
 }
}
#define pulse_45L 226
void turnleft45(int speeda, int speedb) {
   MotorAPos = 0;
   MotorBPos = 0;
   attachInterrupt(digitalPinToInterrupt(MotorAcountA), doMotorA, CHANGE);
   attachInterrupt(digitalPinToInterrupt(MotorBcountA), doMotorB, CHANGE);
   while (MotorAPos > -180 && MotorBPos < 180) {
   zuozhuan(speeda,speedb);
 }
 turnLeftArrive(speed1,speed2);
 turnRightArrive(speed5,speed6);
 detachInterrupt(digitalPinToInterrupt(MotorAcountA));
 detachInterrupt(digitalPinToInterrupt(MotorBcountA));
} 

四. 所有文件

都存在网盘里啦，永久有效。
链接：https://pan.baidu.com/s/1tGJz9DOq5IIM9j1LWF7fFw

---

1. 转自 ：https://bbs.cmnxt.com/forum.php?mod=viewthread&tid=5688&page=1&mobile=2 ↩︎