欧美极品高清xxxxhd,国产日产欧美最新,无码AV国产东京热AV无码,国产精品人与动性XXX,国产传媒亚洲综合一区二区,四库影院永久国产精品,毛片免费免费高清视频,福利所导航夜趣136

標題: 自己寫的stm32小四軸程序 用mpu6050 dmp直接輸出姿態角 裸機代碼 [打印本頁]

---

作者: 南客gggi    時間: 2017-3-21 20:22
標題: 自己寫的stm32小四軸程序 用mpu6050 dmp直接輸出姿態角 裸機代碼
  自己寫的小四軸程序stm32f103c8t6，采用mpu6050中的dmp直接輸出姿態角可不用了解四元數，通行模塊為藍牙模塊，程序是裸機代碼沒有太多變量可以容易看懂，如果有這方面的愛好可以一起探討qq1244450581，PID算法只用了串級PD（這個PID是由模電穩壓的過程得到靈感）不過有引進PID死區，分為倆個PID調節

自己試了感覺挺穩，由于不能發太大文件遙控程序就不發了。




部分源程序預覽：

1. /*******************************************************************************
   
2.                    gggi
   
3.                                                                          四軸飛控程序
   
4.                                                                          由mpu_dmp_get_data(&pitch,&roll,&yaw)這個函數引出歐拉角
   
5.                                                                          由RCDataProcess(&exthrottle,&exroll,&expitch,&exyaw);這個引出期望角與油門
   
6.                                                                         
   
7. *******************************************************************************/
   
8. #include "public.h"
   
9. #include "printf.h"
   
10. #include "stdio.h"
    
11. #include "delay.h"
    
12. #include "dmp.h"
    
13. #include "inv_mpu.h"
    
14. #include "inv_mpu_dmp_motion_driver.h"
    
15. #include "bt.h"
    
16. #include "pwm.h"
    
17. #include "time.h"  //把LED初始化的文件一塊放里面，恩，我沒有偷懶！！！
    
18. #include "pid.h"
    
19.         float old;//用于 判斷是否進入加速劑調節模式
    
20. float exthrottle,roll_ex,pitch_ex,yaw_ex;
    
21. u16 power1,power2,power3,power4;
    
22. //S_FLOAT_ANGLE Q_ANGLE;
    
23. float pitch,roll,yaw;                 //歐拉角
    
24. float gyrox,gyroy,gyroz;
    
25. float Gyrox,Gyroy,Gyroz;
    
26.         float aacx,aacy,aacz;                //加速度傳感器原始數據
    
27. //傳送數據給匿名四軸上位機軟件(V2.6版本)
    
28. //fun:功能字. 0XA0~0XAF
    
29. //data:數據緩存區,最多28字節!!
    
30. //len:data區有效數據個數
    
31. void usart1_niming_report(u8 fun,u8*data,u8 len)
    
32. {
    
33.         u8 send_buf[32];
    
34.         u8 i;
    
35.         if(len>28)return;        //最多28字節數據
    
36.         send_buf[len+3]=0;        //校驗數置零
    
37.         send_buf[0]=0X88;        //幀頭
    
38.         send_buf[1]=fun;        //功能字
    
39.         send_buf[2]=len;        //數據長度
    
40.         for(i=0;i<len;i++)send_buf[3+i]=data[i];                        //復制數據
    
41.         for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf[i];        //計算校驗和        
    
42.         for(i=0;i<len+4;i++)usart1_send_char(send_buf[i]);        //發送數據到串口1
    
43. }        
    
44. //匿名四軸上位機發送的數據格式
    
45. void usart1_report_imu(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz,short roll,short pitch,short yaw)
    
46. {
    
47.         u8 tbuf[28];
    
48.         u8 i;
    
49.         for(i=0;i<28;i++)tbuf[i]=0;//清0
    
50.         tbuf[0]=(aacx>>8)&0XFF;
    
51.         tbuf[1]=aacx&0XFF;
    
52.         tbuf[2]=(aacy>>8)&0XFF;
    
53.         tbuf[3]=aacy&0XFF;
    
54.         tbuf[4]=(aacz>>8)&0XFF;
    
55.         tbuf[5]=aacz&0XFF;
    
56.         tbuf[6]=(gyrox>>8)&0XFF;
    
57.         tbuf[7]=gyrox&0XFF;
    
58.         tbuf[8]=(gyroy>>8)&0XFF;
    
59.         tbuf[9]=gyroy&0XFF;
    
60.         tbuf[10]=(gyroz>>8)&0XFF;
    
61.         tbuf[11]=gyroz&0XFF;        
    
62.         tbuf[18]=(roll>>8)&0XFF;
    
63.         tbuf[19]=roll&0XFF;
    
64.         tbuf[20]=(pitch>>8)&0XFF;
    
65.         tbuf[21]=pitch&0XFF;
    
66.         tbuf[22]=(yaw>>8)&0XFF;
    
67.         tbuf[23]=yaw&0XFF;
    
68.         usart1_niming_report(0XAF,tbuf,28);//飛控顯示幀,0XAF
    
69. }
    
70. 
    
71. int main()
    
72. {               
    
73. 
    
74.          u8 i;
    
75.         float j;//用于遙控油門數值
    
76.         u8 a,b=0,x=0;//用于加速計的累加求平均值
    
77.   u16 flag=0,gyro[3];
    
78. float aac1[10],aac2[10];
    
79. 
    
80. 
    
81.   SystemInit();
    
82.         delay_init(72);
    
83.                 LED_Init() ;
    
84.          printf_init();
    
85.         I2C_GPIO_Config();
    
86.                 delay_ms(10);
    
87.         PIDvalue();//往PID里面塞東西，確定三個姿態的p，i，d值
    
88.         MPU_Init();
    
89.   mpu_dmp_init();
    
90. pwm_init();
    
91. TIM2_Init();
    
92.         
    
93.         
    
94.         while(1)
    
95.         {
    
96. 
    
97.                         mpu_dmp_get_data(&pitch,&roll,&yaw,&gyrox,&gyroy,&gyroz,&aacx,&aacy,&aacz);
    
98.                 ////////////////////////////////////加速劑平滑濾波
    
99. //                aac1[x]=aacx;
    
100. //aac2[x]=aacy;
     
101. //                x++;
     
102. //                for(i=0;i<10;i++)
     
103. //                aacx=(aacx+aac1[i])/2;
     
104. //        
     
105. //                for(i=0;i<10;i++)
     
106. //                aacy=(aacy+aac2[i])/2;
     
107. //                                if(x==10)
     
108. //                        x=0;
     
109.                 /////////////////////////////////////                        
     
110.                                 
     
111.                                 
     
112.                                 
     
113.                                 
     
114.         if(timer==1)
     
115.         {
     
116.         timer=0;
     
117.                         flag++;
     
118. 
     
119.                
     
120.       gyrox=gyrox/10;
     
121.                          gyroy=gyroy/10;
     
122.                          gyroz=gyroz/10;
     
123. //     aacx=aacx/40-25;
     
124. //      aacy=aacy/40+22.5;
     
125. //                aacz=aacz/90-180;
     
126. 
     
127. //       RCDataProcess(&exthrottle,&roll_ex,&pitch_ex,&yaw_ex);
     
128.         
     
129.                
     
130.                 /////////////////////////翻滾程序/////////////////////
     
131. 
     
132.                
     
133.                 /////////////////////////////////////////////////////////////////////////
     
134.                
     
135. 
     
136.                
     
137.                 //////////////////////////////////油門保護程序////////////////////////////////
     
138. //                                        if((old-exthrottle)>400)
     
139. //                        {
     
140. //                                while(1)
     
141. //                                {
     
142. //                                        mpu_dmp_get_data(&pitch,&roll,&yaw,&gyrox,&gyroy,&gyroz);
     
143. //                                         RCDataProcess(&nouse,&roll_ex,&pitch_ex,&yaw_ex);
     
144. //                                exthrottle=old-10;
     
145. //                                singlePIDupdate();
     
146. //                                pwm_in(power1,power2,power3,power4);        
     
147. //                                        if(nouse>300)break;
     
148. //                                }
     
149. //                        }
     
150. //////////////////////////////////////////////////////////////////////////////////////        
     
151.                     RCDataProcess(&exthrottle,&roll_ex,&pitch_ex,&yaw_ex);
     
152. 
     
153. //                                if(roll_ex==170)
     
154. //                {
     
155. //                        pwm_in(power1+200,power2-200,power3-200,power4+200);
     
156. //                        delay_ms(10);
     
157. //                        pwm_in(power1-200,power2+400,power3+400,power4-200);
     
158. //                        delay_ms(10);
     
159. //                        
     
160. //                        power2=power2+400;
     
161. //                        if(power2>1000)
     
162. //                                power2=1000;
     
163. //                                power4=power4-200;
     
164. //                        if(power4<0)
     
165. //                                power4=0;
     
166. //                        
     
167. //                                power3=power3+400;
     
168. //                        if(power3>1000)
     
169. //                                power3=1000;
     
170. //                                power1=power1-200;
     
171. //                        if(power1<0)
     
172. //                                power1=0;
     
173. //                        while(1)
     
174. //                        {
     
175. //                pwm_in(power1,power2,power3,power4);
     
176. //                        mpu_dmp_get_data(&pitch,&roll,&yaw,&gyrox,&gyroy,&gyroz,&aacx,&aacy,&aacz);
     
177. //                                if(roll_ex>150)
     
178. //                                {
     
179. //                                        b=1;
     
180. //                                delay_ms(500);
     
181. //                                }
     
182. //                                if((roll>(-40))&&(b==1))
     
183. //                                {b=0;
     
184. //                                break;
     
185. //                                }
     
186. //                        }
     
187. //                           RCDataProcess(&exthrottle,&roll_ex,&pitch_ex,&yaw_ex);
     
188. //                        mpu_dmp_get_data(&pitch,&roll,&yaw,&gyrox,&gyroy,&gyroz,&aacx,&aacy,&aacz);
     
189. //                              gyrox=gyrox/10;
     
190. //                         gyroy=gyroy/10;
     
191. //                         gyroz=gyroz/10;
     
192. //     aacx=aacx/40-25;
     
193. //      aacy=aacy/40+22.5;
     
194. //                aacz=aacz/90-180;
     
195. //                }
     
196.         //exthrottle=21;
     
197. 
     
198.         if(exthrottle>20)
     
199.         {
     
200.                
     
201. //                        Q_ANGLE.Pitch=(int)((pitch)*100)/100.0;
     
202. //                        Q_ANGLE.Roll=(int)(roll*100)/100.0;
     
203. //                        Q_ANGLE.Yaw=(int)(yaw*100)/100.0;
     
204.                                 singlePIDupdate();   //PID調節
     
205. pwm_in(power1,power2,power3,power4);         //油門輸出
     
206.         
     
207.                 //pwm_in(power1,0,power3,0);         //油門輸出
     
208.         //        pwm_in(0,power2,0,power4);        
     
209. 
     
210.         }
     
211.         else if(exthrottle<20)
     
212.         {
     
213. //                Q_ANGLE.Pitch=(int)((pitch)*100)/100.0+1.5;
     
214. //                Q_ANGLE.Roll=(int)(roll*100)/100.0+0.3;
     
215. //                Q_ANGLE.Yaw=(int)(Q_ANGLE.Yaw*100)/100.0;
     
216.                         pwm_in(0,0,0,0);  //油門輸出
     
217.                
     
218.                
     
219.                
     
220.         }
     
221.         
     
222.         
     
223.         
     
224.         
     
225.                 if(flag==500)
     
226.                 {
     
227.                         led_display1();
     
228.                
     
229.                 //MPU_Get_Accelerometer(&aacx,&aacy,&aacz);        //得到加速度傳感器數據
     
230. 
     
231.                         //usart1_report_imu(exthrottle,roll_ex,pitch_ex,gyrox,gyrox,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));
     
232. //           j=exthrottle+1000;
     
233. //                i[0]=((int)j>>8)&0XFF;
     
234. //                                        i[1]=(int)j&0XFF;
     
235. //                                                        
     
236. 
     
237. //                                usart1_niming_report(0xAE,i,24);
     
238.                
     
239.    }
     
240.                         if(flag==1000)
     
241.                         {
     
242.                                 flag=0;
     
243.                                 led_display2();
     
244.                                  old=exthrottle;
     
245.                                 
     
246.                         }
     
247. 
     
248. 
     
249.         if(pitch>75||pitch<-75||roll<-75||roll>75)
     
250.         {
     
251.                 pwm_in(0,0,0,0);
     
252.                 while(1);
     
253.         }
     
254. //}
     
255. //                        if(flag==30)
     
256. //                {
     
257. //                        //led_display1();
     
258. //               
     
259. //                flag=0;
     
260. 
     
261. //                        usart1_report_imu(exthrottle,roll_ex,pitch_ex,gyrox,gyrox,gyroz,(int)(roll*100),(int)(pitch*100),(int)(yaw*10));
     
262. ////           j=exthrottle+1000;
     
263. ////                i[0]=((int)j>>8)&0XFF;
     
264. ////                                        i[1]=(int)j&0XFF;
     
265. ////                                                        
     
266. 
     
267. ////                                usart1_niming_report(0xAE,i,24);
     
268. //               
     
269.    }
     
270.                
     
271. 
     
272. …………余下代碼請下載附件…………
     

復制代碼

資料下載：
四軸帶遙控.zip (9.82 MB, 下載次數: 46)

2017-3-21 20:21 上傳

點擊文件名下載附件
下載積分: 黑幣 -5

---

作者: 南客gggi    時間: 2017-3-23 17:02
自己試了一下程序穩定性很差，怎么調也調不好，不知道為什么。。

---

歡迎光臨 (http://www.raoushi.com/bbs/)

Powered by Discuz! X3.1

感谢您访问我们的网站，您可能还对以下资源感兴趣：

1024手机基地看电影