Arduino智能小车程序+资料 pid算法

1. #include "pid.h"
   
2. #ifdef ARDUINO_DEBUG
   
3. int debugLeftSpeed;
   
4. int debugRightSpeed;
   
5. uint8_t debugIrs = 0;
   
6. #endif
   
7. const float motorSpeed = 140; //
   
8. const int IR_PIN[] = {A0, A1, A2, A3, A4}; //
   
9. const int IN_A1 = 13;   //
   
10. const int IN_A2 = 12;   //
    
11. const int IN_B1 = 11;  //
    
12. const int IN_B2 = 10;  //
    
13. const int _pwmLeftPin = 5;
    
14. const int _pwmRightPin = 6;
    
15. pid_t pid;
    
16. float pidValue = 0;
    
17. bool turnFlag = false;
    
18. void setup(void)
    
19. {
    
20.   int i;
    
21.   /********************1.初始化电机模块引脚13,12,11,10为输出模式******************/
    
22.   /*
    
23.    *   编写代码
    
24.   */
    
25.     /********************2.设置小车启动速度为140---set_speed()******************/
    
26.   /*
    
27.    *   编写代码
    
28.   */
    
29.   
    
30.   /********************3.初始化循迹模块引脚A0, A1, A2, A3, A4为输入模式******************/
    
31.    /*
    
32.    *   编写代码
    
33.   */
    
34.   //3.设置PID参数
    
35.   pid.sampleTime = SAMPLE_TIME;
    
36.   pid.Kp = KP_VALUE;
    
37.   pid.Ki = KI_VALUE;
    
38.   pid.Kd = KD_VALUE;
    
39.   pid.error = 0;
    
40.   pid.previous_error = 0;
    
41.   Serial.begin(115200);
    
42.   delay(5000);
    
43.   goForward();
    
44.   return;
    
45. }
    
46. 
    
47. //get ir data and middle filter
    
48. uint8_t getIrData(void)  //
    
49. {
    
50.   int i, j;
    
51.   uint8_t level;
    
52.   uint8_t temp;
    
53.   uint8_t irs[9] = {0};//采集9次传感器的传感器的数据
    
54.         //减少取平均值，滤波。
    
55.   for (j = 0; j < 9; j ++) {
    
56.    
    
57.       for (i = 0; i < 5; i++) {
    
58.         level = digitalRead(IR_PIN[i]);
    
59.         if (level) {
    
60.           bitSet(irs[j], i); // 设置irs[j]的第jbit 为1
    
61.         } else {
    
62.           bitClear(irs[j], i); //清除irs[j]的第jbit为0
    
63.         }
    
64.       }
    
65.   }
    
66.   //对irs中的数据，进行9次排序，取最中间的值。
    
67.   for (i = 0; i < 9 - 1; i ++) {
    
68.     for (j = 0; j < 9 - i - 1; j ++) {
    
69.       if (irs[j] > irs[j + 1]) {
    
70.         temp = irs[j];
    
71.         irs[j] = irs[j + 1];
    
72.         irs[j + 1] = temp;
    
73.       }
    
74.     }
    
75.   }
    
76.   
    
77. #ifdef ARDUINO_DEBUG
    
78.   debugIrs = irs[4];
    
79. #endif
    
80.   return irs[4];
    
81. }
    
82. int calcErrorByIrsValue(uint8_t irs)
    
83. {
    
84.   //右边压线为负，左边压线为正。
    
85.   //要左转为负，要右转为正
    
86.   int curError = pid.error; //获得上一次的pid误差值
    
87.   //压线输出低电平0，没有压线输出高电平1
    
88.   switch (irs) { //h'h
    
89.     case B11110: curError = -8; break; //最右边压线
    
90.    
    
91.     case B10000:
    
92.     case B11000: curError = -7; break; //右四个或者右3个沿线
    
93.    
    
94.     case B11100: curError = -6; break; //右两个沿线
    
95.     case B11101: curError = -4; break; //右倒数第二个沿线
    
96.     case B11001: curError = -2; break; //
    
97.    
    
98.     case B00000:       //全部压线和中间压线，对应"十字"弯道，直接郭即可。
    
99.     case B11011: curError = 0;  break;
    
100.    
     
101.     case B10011: curError = 2;  break;
     
102.     case B10111: curError = 4;  break;
     
103.     case B00111: curError = 6;  break;
     
104.    
     
105.     case B00011:
     
106.     case B00001: curError = 7;  break;
     
107.    
     
108.     case B01111: curError = 8;  break;
     
109. //都没有沿线，按照上次的PID值来计算
     
110.     case B11111: curError = pid.error > 0 ? 9 : - 9; break;
     
111.   }
     
112.   return curError;
     
113. }
     
114. 
     
115. void _sortData(int *p, int n)
     
116. {
     
117.   int temp;
     
118.   int i, j;
     
119.   
     
120.   /**编写冒泡排序，对采集到的错误码，进行十次从小到大排序 **/
     
121.   /*
     
122.    *   编写代码
     
123.   */
     
124.   return;
     
125. }
     
126. 
     
127. //计算当前的误差值，得到当前的偏移值
     
128. void calcCurrentError(void)
     
129. {
     
130.   int i;
     
131.   uint8_t irs;
     
132.   float sum = 0;
     
133.   int errorData[10];
     
134.   //1.采集10次循迹模块获得的错误码
     
135.   for (i = 0; i < 10; i ++) {
     
136.     irs =  getIrData();  //得到这次传感器采集的数据（5路数据）
     
137.     errorData[i] =  calcErrorByIrsValue(irs);
     
138.   }
     
139.   //2.要求对errorData中的数据进行排序（从小到大排序）
     
140.     /*
     
141.    *   编写代码
     
142.   */
     
143.   //3.十次中去处一个最高数，去除一个最低数，求中间8次错误码数据的和。
     
144.   /*
     
145.    *   编写代码
     
146.   */
     
147. //4.求8次错误码数据中的偏差。
     
148.   pid.error = sum / 8;
     
149.   return;
     
150. }
     
151. void turnRight(void)
     
152. {
     
153.     digitalWrite(IN_B1, HIGH); //左正
     
154.     digitalWrite(IN_B2, LOW);
     
155.     digitalWrite(IN_A1, LOW); //右反
     
156.     digitalWrite(IN_A2, HIGH);
     
157.    
     
158. }
     
159. void turnLeft(void)
     
160. {
     
161.     digitalWrite(IN_B1, LOW);// 左反
     
162.     digitalWrite(IN_B2, HIGH);
     
163.     digitalWrite(IN_A1, HIGH); //右正
     
164.     digitalWrite(IN_A2, LOW);
     
165.    
     
166. }
     
167. void goForward(void)
     
168. {
     
169.   digitalWrite(IN_B1, HIGH); //左正传
     
170.   digitalWrite(IN_B2, LOW);
     
171.   digitalWrite(IN_A1, HIGH); //右正转
     
172.   digitalWrite(IN_A2, LOW);
     
173. }
     
174. void motorControl(float pidValue, bool turnFlag)
     
175. {
     
176.   int leftMotorSpeed = 0;
     
177.   int rightMotorSpeed = 0;
     
178.   
     
179.   leftMotorSpeed  = constrain((motorSpeed + pidValue), -255, 255);
     
180.   rightMotorSpeed = constrain((motorSpeed - pidValue), -255, 255);
     
181.   if (turnFlag) {
     
182.     if (abs(leftMotorSpeed) > abs(rightMotorSpeed)) {
     
183.       leftMotorSpeed  = abs(leftMotorSpeed);
     
184.       rightMotorSpeed = leftMotorSpeed;
     
185.     } else {
     
186.       rightMotorSpeed =  abs(rightMotorSpeed);
     
187.       leftMotorSpeed  = rightMotorSpeed;
     
188.     }
     
189.   } else {
     
190.     leftMotorSpeed  = leftMotorSpeed  > 0 ? leftMotorSpeed  : -leftMotorSpeed;
     
191.     rightMotorSpeed = rightMotorSpeed > 0 ? rightMotorSpeed : -rightMotorSpeed;
     
192.   }
     
193.   analogWrite(_pwmLeftPin,  leftMotorSpeed );
     
194.   analogWrite(_pwmRightPin, rightMotorSpeed);
     
195. #ifdef ARDUINO_DEBUG
     
196.   debugLeftSpeed  = leftMotorSpeed ;
     
197.   debugRightSpeed = rightMotorSpeed;
     
198. #endif
     
199.   return;
     
200. }
     
201. bool calculatePid(float *pValue)
     
202. {
     
203.   float P = 0;
     
204.   static float I = 0 ;
     
205.   float D = 0 ;
     
206.   static unsigned long lastTime = 0;
     
207.   //获得机器从启动到现在所运行的时间
     
208.   unsigned long now = millis();
     
209.   int timeChange = now - lastTime;//得到这次的时间
     
210.   //若是我们的这次变化的时间，小于pid的
     
211.   //采样时间，就需要等待一下。
     
212.   if (timeChange < pid.sampleTime) {
     
213.     return false;
     
214.   }
     
215.   P = pid.error; //本次小车传感器的偏移大小
     
216.   I = I + pid.error;//历史的偏移
     
217.   D = pid.error - pid.previous_error; //最近两次的差值
     
218.   *pValue = (pid.Kp * P) + (pid.Ki * I) + (pid.Kd * D) + 1;
     
219.   //constrain()函数的功能：若是*pvalue > motorSpeed,取 motorSpeed，
     
220.   //                       若是*pvalue < -motorSpeed,取 -motorSpeed,
     
221.   //        若是-pValue<=*pvalue <= motorSpeed,取*pvalue
     
222.   *pValue = constrain(*pValue, -motorSpeed,motorSpeed);
     
223.   pid.previous_error = pid.error; //记录本次的pid.error，作为下一次的历史差值。
     
224.   lastTime = now; // 记录本次的运行时间，作为下一次时间的历史时间参考。
     
225.   return true;
     
226. }
     
227. 
     
228. void calcDirection(void) //判断小车的转向
     
229. {
     
230.   //pid.error > 0 说明小车左边压线多，要
     
231.   if (pid.error >= 7 && pid.error <= 8) {
     
232.     turnRight();
     
233.     turnFlag = true;
     
234.   } else if (pid.error >= -8 && pid.error <= -7) {
     
235.     turnLeft();
     
236.     turnFlag = true;
     
237.   } else if(pid.error == 9 || pid.error == -9){
     
238.     turnRight();
     
239.     turnFlag = true;
     
240.     }else {
     
241.     goForward();
     
242.     turnFlag = false;
     
243.   }
     
244.   return;
     
245. }
     
246. void loop(void)
     
247. {
     
248.   bool ok;
     
249.   float  pidValue;
     
250.   calcCurrentError();
     
251.   ok = calculatePid(&pidValue); //计算当前pid的差值。，若是就绪了
     
252.   if (ok) {
     
253.     calcDirection();
     
254.     motorControl(pidValue, turnFlag);
     
255.   }
     
256.   return;
     
257. }
     

复制代码

这个要顶

这个好，记录一下

真好，赞一个！

这个资源确实好

做个记号，回头学习一下

这个可以参考

谢谢，学习学习。

编辑不过是什么原因？

arduino如何设置小车启动速度