没有用PID算法的

  1. void getABCD()
  2. { //获取4、5、6、7号灯值
  4.   val4 = 0;
  5.   val5 = 0;
  6.   val6 = 0;
  7.   val7 = 0;
  8.   int i;
  9.   for (i = 0; i < 4; i++) {             //读取四次减小误差
  10.     val4 += analogRead(QI4);
  11.     val5 += analogRead(QI5);
  12.     val7 += analogRead(HI4);
  13.     val6 += analogRead(HI5);
  14.   }
  15.   val4 = val4 / 4;
  16.   val5 = val5 / 4;
  17.   val6 = val6 / 4;
  18.   val7 = val7 / 4;
  19. }
  21. int change = 40 ;
  23. void yanxianzou(int speeda, int speedb) {//沿线走
  24.   int average = 1500;
  25.   //(QvalueMax + QvalueMin )/2，这是经过测试之后的灰度传感器在黑线上的最大值最小值
  26.   getABCD();
  27.   //调用灰度
  28.   if (val4 < average && val5 < average ) { //全黑
  29.     goStraight(speeda,speedb);
  30.   }
  31.   if (val4 < average && val5 > average) { //偏右
  32.     goStraight(speeda + change ,speedb - change );
  33.   }//右轮加速，左轮减速
  34.   if (val4 > average && val5 < average) { //偏左
  35.    goStraight(speeda - change ,speedb + change );
  36.   }//右轮减速，左轮加速
  37.   if (val4 > average && val5 > average) { //全白
  38.     goStraight(speeda,speedb);
  39.   }//右轮保持，左轮保持

用PID算法的需要测试val的值

  1. void getABCD()
  2. { //获取4、5、6、7号灯值
  4.   val4 = 0;
  5.   val5 = 0;
  6.   val6 = 0;
  7.   val7 = 0;
  8.   int i;
  9.   for (i = 0; i < 4; i++) {
  10.     val4 += analogRead(QI4);
  11.     val5 += analogRead(QI5);
  12.     val7 += analogRead(HI4);
  13.     val6 += analogRead(HI5);
  14.   }
  15.   val4 = val4 / 4;
  16.   val5 = val5 / 4;
  17.   val6 = val6 / 4;
  18.   val7 = val7 / 4;
  19. }
  20. void Sbigcircle(int speeda , int speedb )//顺时针走大圆
  21. {
  22.   getABCD();
  23.   int average = (QvalueMax + QvalueMin)/2 ;//1850 //注意写一下最大值是怎么获得的
  24.   P_error1 = val4 - average ;
  25.     if(P_error1 < 0) 
  26.     P_error1 = 0 ;
  28.  // D_error1 = P_error1 - D_error1 ;  //这里的d和i就不调了
  29.  // I_error1 += P_error1 ;
  31.   P_error2 = val5 - average ;
  32.     if(P_error2 < 0) 
  33.     P_error2 = 0 ;
  35.  //D_error2 = P_error2 - D_error2 ;
  36.  //I_error2 += P_error2 ;
  38.   if(val4 < average + 1000 && val5 < average ) //这里需要改
  39.   {
  40.     I_error1 = 0 ;
  41.     I_error2 = 0 ;
  42.   }
  43.   double tem = 0.8 * Kp1;
  45.   if(val4 < 600 && val5 < 600)
  46.   goStraight(speeda - 10 ,speedb + 25);
  48.   else if(val4 < 1000 && val5 > 2000)//偏右   1.3倍Kp稳定
  49.   goStraight(speeda + Kp1 *P_error1 + Ki1 * I_error1+ Kd1 *D_error1   ,speedb - Kp1 *P_error2  - Ki1 * I_error2 - Kd1 *D_error2 );
  51.   else if(val4 > 2000  && val5 < 1000 )//偏左
  52.   goStraight(speeda - Kp1 *P_error1 - Ki1 * I_error1 - Kd1 *D_error1 - 10 ,speedb +  Kp1 *P_error2 + Ki1 * I_error2 + Kd1 *D_error2 + 25);
  53.   else 
  54.   goStraight(speeda - 10 ,speedb + 25 );//25
  55. }

PID改进版

  1. /循迹
  2. const float Kp1 = 13;// Ki1 = 0.25, Kd1 = 1.5;                    //pid前进参数参数 
  3. //const float Kp2 = 11, Ki2 = 0.2, Kd2 = 4.5;                    //pid后退参数参数 
  4. float error = 0, P = 0,;//I = 0, D = 0, 
  5. float PID_value = 0; 
  6. float previous_error = 0;// previous_I = 0;
  8. void getABCD()
  9. { //获取4、5、6、7号灯值
  11.   val4 = 0;
  12.   val5 = 0;
  13.   val6 = 0;
  14.   val7 = 0;
  15.   int i = 0;
  16.   ///滤波
  17.   for (i = 0; i < 4; i++) {
  18.     val4 += analogRead(Qvalue4);
  19.     val5 += analogRead(Qvalue5);
  20.     val6 += analogRead(Hvalue4);
  21.     val7 += analogRead(Hvalue5);
  22.   }
  23.   val4 = val4 / 4;
  24.   val5 = val5 / 4;
  25.   val6 = val6 / 4;
  26.   val7 = val7 / 4;
  28.   if (flag == 1) {//前进flag设为1   //就是说第一种方向行进时的偏差
  29.     if (val4 < 800 && val5 < 600)//在黑线
  30.       error = 0;
  31.     else if (val4 < 800 && val5 > 600)//偏右
  32.       error = 3;
  33.     else if (val4 > 800&& val5 < 600)//偏左
  34.       error = 3;
  35.     else
  36.       error = 0;
  37.   }
  38.   //写一个另外几种方向行进时的偏差
  39.   //else {//后退
  40.     if (val6 < 600 && val7 <1000)//在黑线
  41.       error = 0;
  42.     else if (val6 < 600 && val7 > 1000)//偏右
  43.       error = 3;
  44.     else if (val6 > 600 && val7 < 1000)//偏左
  45.       error = 3;
  46.     else
  47.       error = 0;
  48.   }
  49. }
  51. void calc_pid() {
  52.   P = error;
  53.   //I = I + error;
  54.   //D = error - previous_error;
  56.   if(flag == 1)
  57.   {
  58.       if (I > 30) I = 10;
  59.       PID_value = (Kp1 * P) + (Ki1 * I) + (Kd1* D);
  60.   }
  61.   else 
  62.   {
  63.       if (I > 25) I = 15;
  64.       PID_value = (Kp2 * P) + (Ki2 * I) + (Kd2* D);
  65.   }
  66.   previous_error = error;
  67. }
  69. void yanxianzou(int a, int b) {
  70.   flag = 1;
  71.   getABCD();
  72.   calc_pid();
  73.   if (val4 < 1000 && val5 < 900)//在黑线
  74.     goStraight(a, b);
  75.   else if (val4 < 1000 && val5 > 900)//偏右
  76.     goStraight(a + PID_value, b - PID_value);
  77.   else if (val4 > 1000&& val5 < 900)//偏左
  78.     goStraight(a - PID_value, b + PID_value);
  79.   else
  80.     goStraight(a, b);
  81. }
  83. /*void yanxiantui(int a, int b) {
  84.   flag = 0;
  85.   getABCD();
  86.   calc_pid();
  87.   if (val6 < 800 && val7 <1400)//在黑线
  88.     retreat(a, b);
  89.   else if (val6 >  800  && val7 < 1400)//偏左
  90.     retreat(a - PID_value, b +PID_value);
  91.   else if (val6 <  800  && val7 > 1400)//偏右
  92.     retreat(a+PID_value , b - PID_value);
  93.   else
  94.     retreat(a, b);
  95. }
  96. */