标题: 基于STM32的平衡小车核心算法 [打印本页]

---

作者: kuangleilei    时间: 2020-11-1 17:45
标题: 基于STM32的平衡小车核心算法

1. #include "math.h"
   
2. #include "stdio.h"
   
3. #include "control.h"
   
4. #include "debug.H"
   
5. #include "MPU6050.H"
   
6. #include "communicate.h"
   
7. #include "bsp.h"
   
8. #include "ultrasonic.h"
   
9. #include "infrare.h"
   
10. 
    
11. 
    
12. unsigned char g_u8MainEventCount;
    
13. unsigned char g_u8SpeedControlCount;
    
14. unsigned char g_u8SpeedControlPeriod;
    
15. unsigned char g_u8DirectionControlPeriod;
    
16. unsigned char g_u8DirectionControlCount;
    
17. 
    
18. unsigned char g_cMotorDisable = 0;//值等于0时电机正常转动，否则停止转动
    
19. 
    
20. 
    
21. int g_iGravity_Offset = 0;
    
22. 
    
23. /******电机控制参数******/
    
24. float g_fSpeedControlOut;
    
25. float g_fSpeedControlOutOld;
    
26. float g_fSpeedControlOutNew;
    
27. float g_fAngleControlOut;
    
28. float g_fLeftMotorOut;
    
29. float g_fRightMotorOut;
    
30. 
    
31. /******速度控制参数******/
    
32. 
    
33. short  g_s16LeftMotorPulse;
    
34. short  g_s16RightMotorPulse;
    
35. 
    
36. int  g_s32LeftMotorPulseOld;
    
37. int  g_s32RightMotorPulseOld;
    
38. int  g_s32LeftMotorPulseSigma;
    
39. int  g_s32RightMotorPulseSigma;
    
40. 
    
41. float g_fCarSpeed;
    
42. float g_iCarSpeedSet;
    
43. float g_fCarSpeedOld;
    
44. float g_fCarPosition;
    
45. 
    
46. /*-----角度环和速度环PID控制参数-----*/
    
47. PID_t g_tCarAnglePID={17.0, 0, 23.0};        //*5 /10
    
48. PID_t g_tCarSpeedPID={15.25, 1.08, 0};        //i/10
    
49. /******蓝牙控制参数******/
    
50. float g_fBluetoothSpeed;
    
51. float g_fBluetoothDirection;
    
52. float g_fBluetoothDirectionOld;
    
53. float g_fBluetoothDirectionNew;
    
54. float g_fBluetoothDirectionOut;
    
55. 
    
56. float g_fCarAngle;                 //
    
57. float g_fGyroAngleSpeed;                //                             
    
58. float g_fGravityAngle;                        //
    
59. 
    
60. 
    
61. int g_iLeftTurnRoundCnt = 0;
    
62. int g_iRightTurnRoundCnt = 0;
    
63. 
    
64. static int AbnormalSpinFlag = 0;
    
65. /***************************************************************
    
66. ** 函数名称: CarUpstandInit
    
67. ** 功能描述: 全局变量初始化函数
    
68. ***************************************************************/
    
69. void CarUpstandInit(void)
    
70. {
    
71.         //g_iAccelInputVoltage_X_Axis = g_iGyroInputVoltage_Y_Axis = 0;
    
72.         g_s16LeftMotorPulse = g_s16RightMotorPulse = 0;
    
73.         g_s32LeftMotorPulseOld = g_s32RightMotorPulseOld = 0;
    
74.         g_s32LeftMotorPulseSigma = g_s32RightMotorPulseSigma = 0;
    
75. 
    
76.         g_fCarSpeed = g_fCarSpeedOld = 0;
    
77.         g_fCarPosition = 0;
    
78.         g_fCarAngle    = 0;
    
79.         g_fGyroAngleSpeed = 0;
    
80.         g_fGravityAngle   = 0;
    
81. 
    
82.         g_fAngleControlOut = g_fSpeedControlOut = g_fBluetoothDirectionOut = 0;
    
83.         g_fLeftMotorOut    = g_fRightMotorOut   = 0;
    
84.         g_fBluetoothSpeed  = g_fBluetoothDirection = 0;
    
85.         g_fBluetoothDirectionNew = g_fBluetoothDirectionOld = 0;
    
86. 
    
87.   g_u8MainEventCount=0;
    
88.         g_u8SpeedControlCount=0;
    
89.          g_u8SpeedControlPeriod=0;
    
90. }
    
91. 
    
92. 
    
93. /***************************************************************
    
94. ** 函数名称: AbnormalSpinDetect
    
95. ** 功能描述: 电机转速异常检测      
    
96. ***************************************************************/
    
97. 
    
98. void AbnormalSpinDetect(short leftSpeed,short rightSpeed)
    
99. {
    
100.         static unsigned short count = 0;
     
101.         
     
102.         //速度设置为0时检测，否则不检测
     
103.         if(g_iCarSpeedSet==0)
     
104.         {
     
105.                 if(((leftSpeed>30)&&(rightSpeed>30)&&(g_fCarAngle > -30) && (g_fCarAngle < 30))
     
106.                         ||((leftSpeed<-30)&&(rightSpeed<-30))&&(g_fCarAngle > -30) && (g_fCarAngle < 30))
     
107.                 {// 左右电机转速大于30、方向相同、持续时间超过250ms，且车身角度不超过30度，则判断为悬空空转
     
108.                         count++;
     
109.                         if(count>50){
     
110.                                 count = 0;
     
111.                                 AbnormalSpinFlag = 1;
     
112.                         }
     
113.                 }
     
114.                 else{
     
115.                         count = 0;
     
116.                 }
     
117.         }
     
118.         else{
     
119.                 count = 0;
     
120.         }
     
121. }
     
122. 
     
123. /***************************************************************
     
124. ** 函数名称: LandingDetect
     
125. ** 功能描述: 小车着地检测      
     
126. ***************************************************************/
     
127. void LandingDetect(void)
     
128. {
     
129.         static float lastCarAngle = 0;
     
130.         static unsigned short count = 0,count1 = 0;
     
131.         
     
132.         if(AbnormalSpinFlag == 0)return;
     
133.         
     
134.         // 小车角度5°~-5°启动检测
     
135.         if((g_fCarAngle > -5) && (g_fCarAngle < 5))
     
136.         {
     
137.                 count1++;
     
138.                 if(count1 >= 50)
     
139.                 {//每隔250ms判断一次小车角度变化量，变化量小于0.8°或大于-0.8°判断为小车静止
     
140.                         count1 = 0;
     
141.                         if(((g_fCarAngle - lastCarAngle) < 0.8) && ((g_fCarAngle - lastCarAngle) > -0.8))
     
142.                         {
     
143.                                 count++;
     
144.                                 if(count >= 4){
     
145.                                         count = 0;
     
146.                                         count1 = 0;
     
147.                                         g_fCarPosition = 0;
     
148.                                         AbnormalSpinFlag = 0;
     
149.                                 }
     
150.                         }
     
151.                         else{
     
152.                                 count = 0;
     
153.                         }
     
154.                         lastCarAngle = g_fCarAngle;
     
155.                 }
     
156.         }
     
157.         else
     
158.         {
     
159.                 count1 = 0;
     
160.                 count = 0;
     
161.         }
     
162. }
     
163. 
     
164. /***************************************************************
     
165. ** 函数名称: MotorManage
     
166. ** 功能描述: 电机使能/失能控制      
     
167. ***************************************************************/
     
168. void MotorManage(void)
     
169. {
     
170. 
     
171.         AbnormalSpinDetect(g_s16LeftMotorPulse, g_s16RightMotorPulse);
     
172.                
     
173.         LandingDetect();
     
174.         
     
175.         if(AbnormalSpinFlag)
     
176.         {        
     
177.                 g_cMotorDisable |= (0x01<<1);
     
178.         }
     
179.         else
     
180.         {
     
181.                 g_cMotorDisable &= ~(0x01<<1);
     
182.         }
     
183.         
     
184.         if(g_fCarAngle > 30 || g_fCarAngle < (-30))
     
185.         {
     
186.                 g_cMotorDisable |= (0x01<<2);
     
187.         }
     
188.         else
     
189.         {
     
190.                 g_cMotorDisable &= ~(0x01<<2);
     
191.         }
     
192.         
     
193. }
     
194. 
     
195. /***************************************************************
     
196. ** 函数名称: SetMotorVoltageAndDirection
     
197. ** 功能描述: 电机转速及方向控制函数            
     
198. ***************************************************************/
     
199. void SetMotorVoltageAndDirection(int i16LeftVoltage,int i16RightVoltage)
     
200. {
     
201.           if(i16LeftVoltage<0)
     
202.     {        
     
203.                         GPIO_SetBits(GPIOA, GPIO_Pin_3 );                                    
     
204.       GPIO_ResetBits(GPIOA, GPIO_Pin_4 );
     
205.       i16LeftVoltage = (-i16LeftVoltage);
     
206.     }
     
207.     else
     
208.     {        
     
209.       GPIO_SetBits(GPIOA, GPIO_Pin_4 );                                    
     
210.       GPIO_ResetBits(GPIOA, GPIO_Pin_3 );
     
211.     }
     
212. 
     
213.     if(i16RightVoltage<0)
     
214.     {        
     
215.              GPIO_SetBits(GPIOB, GPIO_Pin_0 );                                    
     
216.       GPIO_ResetBits(GPIOB, GPIO_Pin_1 );
     
217.       i16RightVoltage = (-i16RightVoltage);
     
218.     }
     
219.     else
     
220.     {
     
221.                         GPIO_SetBits(GPIOB, GPIO_Pin_1 );                                    
     
222.                         GPIO_ResetBits(GPIOB, GPIO_Pin_0 );              
     
223.     }
     
224. 
     
225.         if(i16RightVoltage > MOTOR_OUT_MAX)  
     
226.         {
     
227.                 i16RightVoltage = MOTOR_OUT_MAX;
     
228.         }
     
229.         if(i16LeftVoltage > MOTOR_OUT_MAX)
     
230.         {
     
231.            i16LeftVoltage = MOTOR_OUT_MAX;
     
232.         }  
     
233.         
     
234.         if(g_cMotorDisable)
     
235.         {
     
236.                 TIM_SetCompare1(TIM3,0);
     
237.                 TIM_SetCompare2(TIM3,0);
     
238.         }
     
239.         else
     
240.         {
     
241.                 TIM_SetCompare1(TIM3,i16RightVoltage);
     
242.                 TIM_SetCompare2(TIM3,i16LeftVoltage);
     
243.         }
     
244. }
     
245. 
     
246. 
     
247. /***************************************************************
     
248. ** 函数名称: MotorOutput
     
249. ** 功能描述: 电机输出函数
     
250.              将直立控制、速度控制、方向控制的输出量进行叠加,并加
     
251.                          入死区常量，对输出饱和作出处理。
     
252. ***************************************************************/
     
253. void MotorOutput(void)
     
254. {
     
255.         g_fLeftMotorOut  = g_fAngleControlOut - g_fSpeedControlOut - g_fBluetoothDirection ;        
     
256.         g_fRightMotorOut = g_fAngleControlOut - g_fSpeedControlOut + g_fBluetoothDirection ;
     
257. 
     
258. 
     
259.         /*增加死区常数*/
     
260.         if((int)g_fLeftMotorOut>0)       g_fLeftMotorOut  += MOTOR_OUT_DEAD_VAL;
     
261.         else if((int)g_fLeftMotorOut<0)  g_fLeftMotorOut  -= MOTOR_OUT_DEAD_VAL;
     
262.         if((int)g_fRightMotorOut>0)      g_fRightMotorOut += MOTOR_OUT_DEAD_VAL;
     
263.         else if((int)g_fRightMotorOut<0) g_fRightMotorOut -= MOTOR_OUT_DEAD_VAL;
     
264. 
     
265.         /*输出饱和处理，防止超出PWM范围*/                        
     
266.         if((int)g_fLeftMotorOut  > MOTOR_OUT_MAX)        g_fLeftMotorOut  = MOTOR_OUT_MAX;
     
267.         if((int)g_fLeftMotorOut  < MOTOR_OUT_MIN)        g_fLeftMotorOut  = MOTOR_OUT_MIN;
     
268.         if((int)g_fRightMotorOut > MOTOR_OUT_MAX)        g_fRightMotorOut = MOTOR_OUT_MAX;
     
269.         if((int)g_fRightMotorOut < MOTOR_OUT_MIN)        g_fRightMotorOut = MOTOR_OUT_MIN;
     
270.         
     
271.     SetMotorVoltageAndDirection((int)g_fLeftMotorOut,(int)g_fRightMotorOut);
     
272. }
     
273. 
     
274. 
     
275. 
     
276. void GetMotorPulse(void)  //采集电机速度脉冲
     
277. {         
     
278.   g_s16LeftMotorPulse = TIM_GetCounter(TIM2);     
     
279.   g_s16RightMotorPulse= -TIM_GetCounter(TIM4);
     
280.   TIM2->CNT = 0;
     
281.   TIM4->CNT = 0;   //清零
     
282. 
     
283.   g_s32LeftMotorPulseSigma +=  g_s16LeftMotorPulse;
     
284.   g_s32RightMotorPulseSigma += g_s16RightMotorPulse;
     
285.         
     
286.         g_iLeftTurnRoundCnt -= g_s16LeftMotorPulse;
     
287.         g_iRightTurnRoundCnt -= g_s16RightMotorPulse;
     
288. 
     
289. }
     
290. 
     
291. void AngleCalculate(void)
     
292. {
     
293.         //-------加速度--------------------------
     
294.         //量程为±2g时，灵敏度：16384 LSB/g
     
295.     g_fGravityAngle = atan2(g_fAccel_y/16384.0,g_fAccel_z/16384.0) * 180.0 / 3.14;
     
296.           g_fGravityAngle = g_fGravityAngle - g_iGravity_Offset;
     
297. 
     
298.         //-------角速度-------------------------
     
299.         //范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)
     
300.         g_fGyro_x  = g_fGyro_x / 16.4;  //计算角速度值                           
     
301.         g_fGyroAngleSpeed = g_fGyro_x;        
     
302.         
     
303.         //-------互补滤波---------------
     
304.         g_fCarAngle = 0.98 * (g_fCarAngle + g_fGyroAngleSpeed * 0.005) + 0.02 *        g_fGravityAngle;
     
305. }
     
306. /***************************************************************
     
307. ** 函数名称: AngleControl
     
308. ** 功能描述: 角度环控制函数           
     
309. ***************************************************************/
     
310. void AngleControl(void)         
     
311. {
     
312.         g_fAngleControlOut =  (CAR_ANGLE_SET-g_fCarAngle) * g_tCarAnglePID.P *5 + \
     
313.         (CAR_ANGLE_SPEED_SET-g_fGyroAngleSpeed) * (g_tCarAnglePID.D /10);
     
314. }
     
315. 
     
316. 
     
317. 
     
318. /***************************************************************
     
319. ** 函数名称: SpeedControl
     
320. ** 功能描述: 速度环控制函数
     
321. ***************************************************************/
     
322. 
     
323. void SpeedControl(void)
     
324. {
     
325.           float fP,fI;           
     
326.         float fDelta;
     
327.         
     
328.         
     
329.         g_fCarSpeed = (g_s32LeftMotorPulseSigma  + g_s32RightMotorPulseSigma ) * 0.5 ;
     
330.   g_s32LeftMotorPulseSigma = g_s32RightMotorPulseSigma = 0;          //全局变量 注意及时清零
     
331.             
     
332.         g_fCarSpeed = 0.7 * g_fCarSpeedOld + 0.3 * g_fCarSpeed ;//低通滤波，使速度更平滑
     
333.         g_fCarSpeedOld = g_fCarSpeed;
     
334. 
     
335.         fDelta = CAR_SPEED_SET;
     
336.         fDelta -= g_fCarSpeed;   
     
337.         
     
338.         fP = fDelta * (g_tCarSpeedPID.P);
     
339.   fI = fDelta * (g_tCarSpeedPID.I/10.0);
     
340. 
     
341.         g_fCarPosition += fI;
     
342.         g_fCarPosition += g_fBluetoothSpeed;         
     
343.         
     
344. //积分上限设限
     
345.         if((s16)g_fCarPosition > CAR_POSITION_MAX)    g_fCarPosition = CAR_POSITION_MAX;
     
346.         if((s16)g_fCarPosition < CAR_POSITION_MIN)    g_fCarPosition = CAR_POSITION_MIN;
     
347.         
     
348.         g_fSpeedControlOutOld = g_fSpeedControlOutNew;
     
349.   g_fSpeedControlOutNew = fP + g_fCarPosition;
     
350. }
     
351. /***************************************************************
     
352. ** 函数名称: SpeedControlOutput
     
353. ** 功能描述: 速度环控制输出函数-分多步逐次逼近最终输出，尽可能将对直立环的干扰降低。
     
354. ***************************************************************/
     
355. void SpeedControlOutput(void)
     
356. {
     
357.   float fValue;
     
358.   fValue = g_fSpeedControlOutNew - g_fSpeedControlOutOld ;
     
359.   g_fSpeedControlOut = fValue * (g_u8SpeedControlPeriod + 1) / SPEED_CONTROL_PERIOD + g_fSpeedControlOutOld;
     
360. }
     
361. 
     
362. 
     
363. /***************************************************************
     
364. ** 函数名称: Scale
     
365. ** 功能描述: 量程归一化处理
     
366. ***************************************************************/
     
367. float Scale(float input, float inputMin, float inputMax, float outputMin, float outputMax) {
     
368.   float output;
     
369.   if (inputMin < inputMax)
     
370.     output = (input - inputMin) / ((inputMax - inputMin) / (outputMax - outputMin));
     
371.   else
     
372.     output = (inputMin - input) / ((inputMin - inputMax) / (outputMax - outputMin));
     
373.   if (output > outputMax)
     
374.     output = outputMax;
     
375.   else if (output < outputMin)
     
376.     output = outputMin;
     
377.   return output;
     
378. }
     
379. 
     
380. /***************************************************************
     
381. ** 函数名称: Steer
     
382. ** 功能描述: 遥控速度及方向处理函数
     
383. ***************************************************************/
     
384. void Steer(float direct, float speed)
     
385. {
     
386.         if(direct > 0)
     
387.                 g_fBluetoothDirection = Scale(direct, 0, 10, 0, 400);
     
388.         else
     
389.                 g_fBluetoothDirection = -Scale(direct, 0, -10, 0, 400);
     
390. 
     
391.         if(speed > 0)
     
392.                 g_iCarSpeedSet = Scale(speed, 0, 10, 0, 70);
     
393.         else
     
394.                 g_iCarSpeedSet = -Scale(speed, 0, -10, 0, 70);
     
395. 
     
396. }
     
397. 
     
398. void UltraControl(int mode)
     
399. {
     
400.         if(mode == 0)
     
401.         {
     
402.                 if((Distance >= 0) && (Distance<= 12))
     
403.                 {//距离小于12cm则后退
     
404.                         Steer(0, -4);
     
405.                 }
     
406.                 else if((Distance> 18) && (Distance<= 30))        
     
407.                 {//距离大于18cm且小于30则前进
     
408.                         Steer(0, 4);
     
409.                 }
     
410.                 else
     
411.                         Steer(0, 0);
     
412.         }
     
413.         else if(mode == 1)
     
414.         {
     
415.                 if((Distance >= 0) && (Distance<= 20))
     
416.                 {//右转750个脉冲计数，转弯角度约为90度
     
417.                         Steer(5, 0);
     
418.                         g_iLeftTurnRoundCnt = 750;
     
419.                         g_iRightTurnRoundCnt = -750;
     
420.                 }
     
421.                 if((g_iLeftTurnRoundCnt < 0)&&(g_iRightTurnRoundCnt > 0))
     
422.                 {
     
423.                         Steer(0, 4);
     
424.                 }
     
425.         }
     
426. }
     
427. 
     
428. /***************************************************************
     
429. ** 函数名称: TailingControl
     
430. ** 功能描述: 红外寻迹           
     
431. ***************************************************************/
     
432. void TailingControl(void)
     
433. {
     
434. #if INFRARE_DEBUG_EN > 0
     
435.         char buff[32];        
     
436. #endif
     
437.         char result;
     
438.         float direct = 0;
     
439.         float speed = 0;
     
440. 
     
441.         result = InfraredDetect();
     
442.         
     
443.         if(result & infrared_channel_Lc)
     
444.                 direct = -10;
     
445.         else if(result & infrared_channel_Lb)
     
446.                 direct = -6;
     
447.         else if(result & infrared_channel_La)
     
448.                 direct = -4;
     
449.         else if(result & infrared_channel_Rc)
     
450.                 direct = 10;
     
451.         else if(result & infrared_channel_Rb)
     
452.                 direct = 6;
     
453.         else if(result & infrared_channel_Ra)
     
454.                 direct = 4;
     
455.         else
     
456.                 direct = 0.0;
     
457. 
     
458.         speed = 3;
     
459. 
     
460.         Steer(direct, speed);
     
461. 
     
462. #if INFRARE_DEBUG_EN > 0
     
463.         sprintf(buff, "Steer:%d, Speed:%d\r\n",(int)direct,  (int)speed);
     
464.         DebugOutStr(buff);
     
465. #endif
     
466. }
     
467. 
     

复制代码

---

欢迎光临 (http://www.51hei.com/bbs/)

Powered by Discuz! X3.1