标题: STM32平衡小车的代码 PID参数的调节和陀螺仪原始数据的处理 [打印本页]

---

作者: 133456789    时间: 2020-12-29 12:34
标题: STM32平衡小车的代码 PID参数的调节和陀螺仪原始数据的处理
主要是PID参数的调节和陀螺仪原始数据的处理
远离DMP这种速度慢消耗大的方法，希望有点帮助

单片机源程序如下:

1. #include "led.h"
   
2. #include "delay.h"
   
3. #include "key.h"
   
4. #include "sys.h"
   
5. #include "lcd.h"
   
6. #include "usart.h"
   
7. #include "mpu6050.h"   
   
8. #include "inv_mpu.h"
   
9. #include "inv_mpu_dmp_motion_driver.h"
   
10. #include "oled.h"
    
11. #include "pwm.h"
    
12. #include "function.h"
    
13. #include "fuzzy.h"
    
14. #include "time.h"
    
15. #include "FTM.h"
    
16. #include "rtu.h"
    
17. 
    
18. char receive;
    
19. float result;
    
20. angle ress;
    
21. p_angle res;
    
22. speed_s spe_L;
    
23. speed_ss speed_L;
    
24. speed_s spe_R;
    
25. speed_ss speed_R;
    
26. out_b outt;
    
27. out_c out;
    
28. int ab;
    
29. float roll;
    
30. int ti=9;
    
31. float ta;
    
32. int ti_r;
    
33. int ti_l;
    
34. float CN_timer2;
    
35. float CN_timer3;
    
36. int time;
    
37. int32_t rtu_send[6];
    
38. int main(void)
    
39. {       
    
40.         float mm=0;
    
41.         int P=64;
    
42.         float I=0;
    
43.         float D=0.005;
    
44.         float i;
    
45.         float d;
    
46.         float e;
    
47.         int integer,point;
    
48.         float pitch,yaw;                 //欧拉角
    
49.         short aacx,aacy,aacz;                  //加速度传感器原始数据
    
50.         short gyrox,gyroy,gyroz;        //陀螺仪原始数据
    
51.         short temp;                                              //温度       
    
52.         int m;
    
53.   out=&outt;
    
54.   res=&ress;
    
55.         speed_L=&spe_L;
    
56.         speed_R=&spe_R;
    
57.         speed_L->speed_last=0;
    
58.         speed_R->speed_last=0;
    
59.         ta=0;
    
60.         ti=80;
    
61.         res->angle_sum=0;
    
62.         res->angle_error=0;
    
63.         res->angle_last=0;
    
64.   res->angle_now=0;
    
65.         m=0;
    
66.   delay_init();        //延时初始化
    
67.         OLED_Init();       
    
68.         pwm_init();
    
69.         time1_init();
    
70.         uart_init(115200);
    
71.         Hardware_init();
    
72.         ti=0;
    
73.        
    
74.   ab=1000;
    
75.   OLED_ShowString(40,2 ,"Stop");
    
76.         while(1)
    
77.         {
    
78. //                if(ti<0)  
    
79. //                          ti=0;
    
80. //          
    
81. //                if(ab>100)
    
82. //                        ab=100;
    
83. //                  ti=ab;
    
84. //       
    
85.                 if(ti<0)  ti=0;
    
86.                 if(ti>100)  ti=100;
    
87.                 if(ab<0)   ab=0;
    
88.                 if(ab>0)   ab=100;
    
89.                
    
90.                         OLED_ShowString(0,0 ,"Direction:");
    
91.                 OLED_ShowString(0,4 ,"Speed:");
    
92.                 OLED_ShowNum(40,6,100-ti,3,24);
    
93.                 OLED_ShowString(96,6 ,"%");
    
94.                 //  OLED_ShowNum(0,2,ta,3,32);
    
95.    switch (receive)
    
96.                         {
    
97.                                 case 'A':
    
98.                                             ti+=5;
    
99.                                       receive='p';
    
100.                                          break;
     
101.                            case 'B':
     
102.                                             ti-=5;
     
103.                                       receive='p';
     
104.                                       break;
     
105.                                 case 'C':
     
106.                                             forward();
     
107.                                       OLED_Clear();
     
108.                                             OLED_ShowString(40,2 ,"Forward");
     
109.                                       receive='p';
     
110.                                       break;
     
111.                                         case 'D':
     
112.                                             back();
     
113.                                       OLED_Clear();
     
114.                                             OLED_ShowString(40,2 ,"Back");
     
115.                                       receive='p';
     
116.                                       break;
     
117.                                         case 'E' :
     
118.                                                   stop();
     
119.                                             ti=0;;
     
120.                                             OLED_Clear();
     
121.                                             OLED_ShowString(40,2 ,"Stop");
     
122.                                             receive='p';
     
123.                                       break;
     
124.                                   case 'F' :
     
125.                                                   left();
     
126.                                             OLED_Clear();
     
127.                                             OLED_ShowString(40,2 ,"Turn_left");
     
128.                                             receive='p';
     
129.                                       break;
     
130.                                         case 'G' :
     
131.                                                   right();
     
132.                                             OLED_Clear();
     
133.                                             OLED_ShowString(40,2 ,"Turn_right");
     
134.                                             receive='p';
     
135.                                       break;
     
136.                                                 case 'H' :
     
137.                                                   up();
     
138.                                                 ti=50;
     
139.                                             OLED_Clear();
     
140.                                             OLED_ShowString(40,2 ,"up");
     
141.                                             receive='p';
     
142.                                       break;
     
143.                                                 case 'I' :
     
144.                                                   down();
     
145.                                                 ti=50;
     
146.                                             OLED_Clear();
     
147.                                             OLED_ShowString(40,2 ,"down");
     
148.                                             receive='p';
     
149.                                       break;
     
150.                                                 case 'J' :
     
151.                                                   ab-=5;
     
152.                                             OLED_Clear();
     
153.                                             receive='p';
     
154.                                       break;
     
155.                                                 case 'K' :
     
156.                                                   ab+=5;
     
157.                                             OLED_Clear();
     
158.                                             receive='p';
     
159.                                       break;
     
160.                                        
     
161.                         }
     
162.         }
     
163.         Hardware_init();       
     
164.         uart_init(115200);
     
165.         //time4_init();
     
166.         usart_init_();
     
167.         time1_init();
     
168.         Encoder_Init_TIM2();
     
169.         Encoder_Init_TIM3();
     
170.         while(mpu_dmp_init())
     
171.         {
     
172.         OLED_ShowString(0,2 ,"MPU6050 initting");
     
173.         }
     
174.   OLED_Clear();
     
175.         res->angle_P=81;
     
176.         res->angle_I=0.001;
     
177.         res->angle_sum=0;
     
178.         res->angle_D=18;
     
179.         mm=0;
     
180.         OLED_ShowString(0,2 ,"roll:");
     
181.         OLED_ShowString(2,0 ,"L:");
     
182.         OLED_ShowString(60,0 ,"R:");         
     
183.         OLED_ShowString(100,2 ,".");
     
184.         OLED_ShowString(0,6 ,"P:");
     
185.         OLED_ShowString(46,6,"I:");       
     
186.         OLED_ShowString(90,6 ,"D:");
     
187.                          while(1)
     
188.         {
     
189.                   switch (receive)
     
190.                         {
     
191.                                 case 'T':
     
192.                                             ta=ta+1;
     
193.                                       receive='p';
     
194.                                          break;
     
195.                                 case 'J':
     
196.                                               ta-=1;
     
197.                                       receive='p';
     
198.                                          break;   
     
199.         case 'A':
     
200.               res->angle_P+=1;
     
201.                receive='p';       
     
202.                break;
     
203.                                 case 'C':
     
204.               res->angle_P-=1        ;
     
205.                receive='p';
     
206.                break;
     
207.          case 'D':
     
208.               res->angle_D+=1;
     
209.                receive='p';       
     
210.                break;
     
211.                                 case 'E':
     
212.                res->angle_D-=1;
     
213.                receive='p';
     
214.                break;                       
     
215.                                  case 'F' :
     
216.                          res->angle_I+=0.001;
     
217.                                        receive = 'p';
     
218.                            break;
     
219.                     case 'B':
     
220.                     back();
     
221.                                      res->angle_I-=0.001;
     
222.                            break;
     
223.                      case 'S':
     
224.                                              stop();
     
225.                                               receive = 'p';
     
226.                            break;         
     
227.                                 case 'M':
     
228.                      ti+=1;
     
229.                                        receive = 'p';
     
230.                            break;
     
231.                      case 'N':
     
232.                                               receive = 'p';
     
233.                            ti-=1;
     
234.                            break;       
     
235.                                
     
236.                         }
     
237.            if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
     
238.                  {       
     
239.                         MPU_Get_Accelerometer(&aacx,&aacy,&aacz);        //得到加速度传感器数据
     
240.                         MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);        //得到陀螺仪数据         
     
241.                   temp=roll*10;         
     
242.                 if(temp<0)
     
243.                                   {
     
244.                                      OLED_ShowString(50,2 ,"-");
     
245.                                                  temp=-temp;
     
246.                                   } else               
     
247.                                           OLED_ShowString(50,2 ,"+");
     
248.         integer = temp/10;
     
249.         point   = temp%10;       
     
250.                 time=(2000-(res->angle_P*mm+res->angle_I*res->angle_sum-res->angle_D*res->angle_error));
     
251.         if(time<1)
     
252.                                  {
     
253.                                                 time=1;
     
254.                                  }                                                 
     
255.                 if(res->angle_now<0)
     
256.                 {
     
257.              forward();
     
258.                          mm=-res->angle_now;
     
259.                  }
     
260.                  else
     
261.                  {       
     
262.                            back();
     
263.                            mm=res->angle_now;
     
264.                  }
     
265.                  OLED_ShowNum(60,2,integer,3,18);
     
266.                           OLED_ShowNum(108,2,point,1,8);       
     
267.                     OLED_ShowNum(12,6,(res->angle_P),3,18);
     
268.                     OLED_ShowNum(60,6,(res->angle_I)*1000,3,18);
     
269.         OLED_ShowNum(102,6,res->angle_D,3,18);
     
270.                           OLED_ShowNum(0,4,mm,3,18);               
     
271.                 if(res->angle_error<0.5)
     
272.                 {
     
273.                       OLED_ShowString(68,4 ,"-");
     
274.                       OLED_ShowNum(76,4,-res->angle_error,4,20);       
     
275.                 }
     
276.                         else
     
277.                         {
     
278.                             OLED_ShowString(68,4 ,"+");
     
279.                             OLED_ShowNum(76,4,res->angle_error,4,20);
     
280.                         }                               
     
281. //                    OLED_ShowNum(20,0,(int)((s16)(TIM2->CNT)),3,16);         
     
282. //                    OLED_ShowNum(80,0,(int)((s16)(TIM3->CNT)),3,16);          //显示左右轮位置
     
283.                         rtu_send[0]=res->angle_now*100;
     
284.                         rtu_send[1]=2000-time;
     
285.                         rtu_send_data(rtu_send,2);
     
286.                   rtu_send_data(rtu_send,2);                    //上位机发送
     
287.                         OLED_ShowNum(20,0,time,4,20);                    //显示左右论速度
     
288.                   OLED_ShowNum(80,0,time,4,20);
     
289.                         while(1)
     
290.                         {
     
291.             GPIO_ResetBits(GPIOC,GPIO_Pin_13);
     
292.                                         delay_ms(1000);                        //显示左右论速度
     
293.                         GPIO_SetBits(GPIOC,GPIO_Pin_13);
     
294.                         delay_ms(1000);
     
295.                         }
     
296.                  }
     
297.         }
     
298. 
     
299. }
     

复制代码

所有资料51hei提供下载:
两轮平衡车.7z (257.04 KB, 下载次数: 56)

2020-12-30 03:38 上传

点击文件名下载附件
下载积分: 黑币 -5

---

作者: yimaer    时间: 2024-7-30 14:07
请问，为什么主函数里有2个while（1）呢？ switch (receive)，receive变量没给初值，怎么进的case语句呢

---

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

Powered by Discuz! X3.1