標(biāo)題: 自己寫的stm32小四軸程序 用mpu6050 dmp直接輸出姿態(tài)角 裸機(jī)代碼 [打印本頁(yè)]
作者: 南客gggi    時(shí)間: 2017-3-21 20:22
標(biāo)題: 自己寫的stm32小四軸程序 用mpu6050 dmp直接輸出姿態(tài)角 裸機(jī)代碼
  自己寫的小四軸程序stm32f103c8t6，采用mpu6050中的dmp直接輸出姿態(tài)角可不用了解四元數(shù)，通行模塊為藍(lán)牙模塊，程序是裸機(jī)代碼沒(méi)有太多變量可以容易看懂，如果有這方面的愛(ài)好可以一起探討qq1244450581，PID算法只用了串級(jí)PD（這個(gè)PID是由模電穩(wěn)壓的過(guò)程得到靈感）不過(guò)有引進(jìn)PID死區(qū)，分為倆個(gè)PID調(diào)節(jié)

自己試了感覺(jué)挺穩(wěn)，由于不能發(fā)太大文件遙控程序就不發(fā)了。




部分源程序預(yù)覽：
  1. /*******************************************************************************
  2.                    gggi
  3.                                                                          四軸飛控程序
  4.                                                                          由mpu_dmp_get_data(&pitch,&roll,&yaw)這個(gè)函數(shù)引出歐拉角
  5.                                                                          由RCDataProcess(&exthrottle,&exroll,&expitch,&exyaw);這個(gè)引出期望角與油門
  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初始化的文件一塊放里面，恩，我沒(méi)有偷懶�。�！
  18. #include "pid.h"
  19.         float old;//用于 判斷是否進(jìn)入加速劑調(diào)節(jié)模式
  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;                //加速度傳感器原始數(shù)據(jù)
  27. //傳送數(shù)據(jù)給匿名四軸上位機(jī)軟件(V2.6版本)
  28. //fun:功能字. 0XA0~0XAF
  29. //data:數(shù)據(jù)緩存區(qū),最多28字節(jié)!!
  30. //len:data區(qū)有效數(shù)據(jù)個(gè)數(shù)
  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字節(jié)數(shù)據(jù)
  36.         send_buf[len+3]=0;        //校驗(yàn)數(shù)置零
  37.         send_buf[0]=0X88;        //幀頭
  38.         send_buf[1]=fun;        //功能字
  39.         send_buf[2]=len;        //數(shù)據(jù)長(zhǎng)度
  40.         for(i=0;i<len;i++)send_buf[3+i]=data[i];                        //復(fù)制數(shù)據(jù)
  41.         for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf[i];        //計(jì)算校驗(yàn)和        
  42.         for(i=0;i<len+4;i++)usart1_send_char(send_buf[i]);        //發(fā)送數(shù)據(jù)到串口1
  43. }        
  44. //匿名四軸上位機(jī)發(fā)送的數(shù)據(jù)格式
  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. }

  71. int main()
  72. {               

  74.          u8 i;
  75.         float j;//用于遙控油門數(shù)值
  76.         u8 a,b=0,x=0;//用于加速計(jì)的累加求平均值
  77.   u16 flag=0,gyro[3];
  78. float aac1[10],aac2[10];


  81.   SystemInit();
  82.         delay_init(72);
  83.                 LED_Init() ;
  84.          printf_init();
  85.         I2C_GPIO_Config();
  86.                 delay_ms(10);
  87.         PIDvalue();//往PID里面塞東西，確定三個(gè)姿態(tài)的p，i，d值
  88.         MPU_Init();
  89.   mpu_dmp_init();
  90. pwm_init();
  91. TIM2_Init();
  92.         
  93.         
  94.         while(1)
  95.         {

  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++;

  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;

  127. //       RCDataProcess(&exthrottle,&roll_ex,&pitch_ex,&yaw_ex);
  128.         
  129.                
  130.                 /////////////////////////翻滾程序/////////////////////

  132.                
  133.                 /////////////////////////////////////////////////////////////////////////
  134.                

  136.                
  137.                 //////////////////////////////////油門保護(hù)程序////////////////////////////////
  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);

  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;

  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調(diào)節(jié)
  205. pwm_in(power1,power2,power3,power4);         //油門輸出
  206.         
  207.                 //pwm_in(power1,0,power3,0);         //油門輸出
  208.         //        pwm_in(0,power2,0,power4);        

  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);        //得到加速度傳感器數(shù)據(jù)

  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. //                                                        

  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.                         }


  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;

  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. ////                                                        

  267. ////                                usart1_niming_report(0xAE,i,24);
  268. //               
  269.    }
  270.                

  272. …………余下代碼請(qǐng)下載附件…………
復(fù)制代碼

資料下載：
四軸帶遙控.zip (9.82 MB, 下載次數(shù): 46)
作者: 南客gggi    時(shí)間: 2017-3-23 17:02
自己試了一下程序穩(wěn)定性很差，怎么調(diào)也調(diào)不好，不知道為什么。。



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1