標(biāo)題: 四軸四元數(shù)的51單片機(jī)源程序 [打印本頁(yè)]
作者: 1032995173    時(shí)間: 2017-5-16 20:14
標(biāo)題: 四軸四元數(shù)的51單片機(jī)源程序
分享個(gè)四軸四元數(shù)的51單片機(jī)代碼：



單片機(jī)源程序如下:
  1. //四川 綿陽(yáng)      西南科技大學(xué) 信息工程學(xué)院 電氣13級(jí) 劉其民  
  2. //硬件參數(shù)：
  3. //電池:1S/3.7V電池 推薦300-500mAh左右
  4. //電機(jī)/槳:720空心杯/59MM槳
  5. //MCU IAP15W4K61S4@28.000MHZ  
  6. //陀螺儀加速度計(jì)：MPU-6050
  7. //磁場(chǎng)傳感計(jì):HMC5883L
  8. //無線芯片:NRF24L01
  9. //電機(jī)驅(qū)動(dòng)MOS管:AO3400
  10. //MOS管保護(hù)用肖特基:BAT54ST
  11. //下載口保護(hù)用瞬態(tài)抑制二極管:RCLAMP0524P
  12. //機(jī)架尺寸:94mm*94mm
  13. //標(biāo)準(zhǔn)PID控制公式:PID=P*e(n)+I*[(e(n)+e(n-1)+...+e(0)]+D*[e(n)-e(n-1)]

  15. //數(shù)據(jù)定義說明：
  16. //data 51單片機(jī)片內(nèi)RAM最前面128字節(jié)RAM 用ACC讀寫，速度最快
  17. //idata 片內(nèi)RAM最前面256字節(jié)的RAM 包括data 用類似指針模式訪問 適合用于指針操作
  18. //pdata 外部擴(kuò)展RAM的前256字節(jié)的RAM
  19. //xdata 外部擴(kuò)展RAM 用DPTR訪問
  20. #include <STC15W4K60S4.H>
  21. #include <intrins.h>
  22. #include <NRF24L01.H>
  23. #include <MPU6050.H>
  24. #include <math.h>
  25. #include <STC15W4KPWM.H>
  26. void Time0_Init();   //定時(shí)器初始化
  27. void update();       //陀螺儀矯正
  28. void Kalman_Filter(float Accel,float Gyro);   //X軸卡爾曼濾波
  29. void Kalman_Filter_Y(float Accel,float Gyro);        //Y軸卡爾曼濾波       
  30. //*************************************自整定PID相關(guān)參數(shù)************************************************
  31. //unsigned char xdata ERRORPID[512]={0};
  32. //unsigned char xdata ZSYPID[100]={0};
  33. //unsigned long int xdata ALLERROR[100]={0};
  34. //unsigned long int TE=0,TZA=0,ADD=0,THEEND=0,LESTERROR=0;
  35. //float BESTPID=0;
  36. //******************************************************************************************************
  37. unsigned char PID=0;                         //無線串口PID調(diào)整延時(shí)用
  38. int ich1=0,ich2=0,ich3=0,ich4=0;                                     //無線串口待發(fā)送數(shù)據(jù)
  39. int speed0=0,speed1=0,speed2=0,speed3=0,V=0; //電機(jī)控制參數(shù)
  40. int PWM0=0,PWM1=0,PWM2=0,PWM3=0;             //電機(jī)控制參數(shù)
  41. unsigned char TxBuf[20]={0};
  42. unsigned char RxBuf[20]={0};
  43. double g_x=0,g_y=0,g_z=0;                    //陀螺儀靜差消除參數(shù)
  44. double PID_x=0,PID_y=0,PID_z=0;              //每根軸PID控制量輸出
  45. //******角度參數(shù)****************************************************************************************
  46. double Angle_ax=0,Angle_ay=0,Angle_az=0;    //由加速度計(jì)算的傾斜角度
  47. double Angle_gy=0,Angle_gx=0,Angle_gz=0;    //由角速度計(jì)算的傾斜角度
  48. double AngleAx=0,AngleAy=0;                 //反三角函數(shù)處理后的角度值
  49. double Angle=0,Angley=0;                    //最終傾斜角度
  50. double Anglelate=0,Anglelatey=0,Anglezlate=0;//存儲(chǔ)上一次角度數(shù)據(jù)
  51. double Ax=0,Ay=0;                            //加入遙控器控制量后的角度
  52. float idata dt=0.008;                              //系統(tǒng)周期
  53. //******卡爾曼濾波參數(shù)-X軸******************************************************************************
  54. float data Q_angle=0.001;     //對(duì)加速度計(jì)的信任度
  55. float data Q_gyro=0.003;      //對(duì)陀螺儀的信任度
  56. float data R_angle=0.5;
  57. char  data C_0 = 1;
  58. float data Q_bias, Angle_err;
  59. float data PCt_0, PCt_1, E;
  60. float data K_0, K_1, t_0, t_1;
  61. float data Pdot[4] ={0,0,0,0};
  62. float data PP[2][2] = { { 1, 0 },{ 0, 1 } };
  63. void Kalman_Filter(float Accel,float Gyro)               
  64. {
  65.         Angle+=(Gyro - Q_bias) * dt; //陀螺儀積分角度（測(cè)量值-陀螺儀偏差)*dt  
  66. //Angle相當(dāng)于是系統(tǒng)的預(yù)測(cè)值
  67.         Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // 先驗(yàn)估計(jì)誤差協(xié)方差的微分
  68.         Pdot[1]=- PP[1][1];
  69.         Pdot[2]=- PP[1][1];
  70.         Pdot[3]=  Q_gyro;
  71.         PP[0][0] += Pdot[0] * dt;   // 先驗(yàn)估計(jì)誤差協(xié)方差微分的積分
  72.         PP[0][1] += Pdot[1] * dt;   
  73.         PP[1][0] += Pdot[2] * dt;
  74.         PP[1][1] += Pdot[3] * dt;
  75.         Angle_err = Accel - Angle;//估計(jì)值與預(yù)測(cè)值之間的偏差
  76.         PCt_0 = C_0 * PP[0][0];
  77.         PCt_1 = C_0 * PP[1][0];
  78.         E = R_angle + C_0 * PCt_0;
  79.         K_0 = PCt_0 / E;//卡爾曼增益1 用于計(jì)算最優(yōu)估計(jì)值
  80.         K_1 = PCt_1 / E;//卡爾曼增益2 用于計(jì)算最后估計(jì)的偏差
  81.         t_0 = PCt_0;
  82.         t_1 = C_0 * PP[0][1];
  83.         PP[0][0] -= K_0 * t_0;                 //更新協(xié)方差矩陣
  84.         PP[0][1] -= K_0 * t_1;
  85.         PP[1][0] -= K_1 * t_0;
  86.         PP[1][1] -= K_1 * t_1;
  87.         Angle        += K_0 * Angle_err;//根據(jù)卡爾曼增益1算出最優(yōu)角度
  88.         Q_bias        += K_1 * Angle_err;//根據(jù)卡爾曼增益2算出預(yù)測(cè)值偏差
  89. }
  90. //******卡爾曼濾波參數(shù)-Y軸***************************************************************************
  91.                
  92. float data Q_angley=0.001;  
  93. float data Q_gyroy=0.003;
  94. float data R_angley=0.5;
  95. char  data C_0y = 1;
  96. float idata Q_biasy, Angle_erry;
  97. float idata PCt_0y, PCt_1y, Ey;
  98. float idata K_0y, K_1y, t_0y, t_1y;
  99. float idata Pdoty[4] ={0,0,0,0};
  100. float idata PPy[2][2] = { { 1, 0 },{ 0, 1 } };

  102. void Kalman_Filter_Y(float Accely,float Gyroy)               
  103. {
  104.         Angley+=(Gyroy - Q_biasy) * dt;
  105.         Pdoty[0]=Q_angley - PPy[0][1] - PPy[1][0];
  106.         Pdoty[1]=- PPy[1][1];
  107.         Pdoty[2]=- PPy[1][1];
  108.         Pdoty[3]=Q_gyroy;
  109.         PPy[0][0] += Pdoty[0] * dt;   
  110.         PPy[0][1] += Pdoty[1] * dt;   
  111.         PPy[1][0] += Pdoty[2] * dt;
  112.         PPy[1][1] += Pdoty[3] * dt;
  113.         Angle_erry = Accely - Angley;       
  114.         PCt_0y = C_0y * PPy[0][0];
  115.         PCt_1y = C_0y * PPy[1][0];
  116.         Ey = R_angley + C_0y * PCt_0y;
  117.         K_0y = PCt_0y / Ey;
  118.         K_1y = PCt_1y / Ey;
  119.         t_0y = PCt_0y;
  120.         t_1y = C_0y * PPy[0][1];
  121.         PPy[0][0] -= K_0y * t_0y;               
  122.         PPy[0][1] -= K_0y * t_1y;
  123.         PPy[1][0] -= K_1y * t_0y;
  124.         PPy[1][1] -= K_1y * t_1y;
  125.         Angley        += K_0y * Angle_erry;       
  126.         Q_biasy        += K_1y * Angle_erry;       
  127. }
  128. //************姿態(tài)處理和PID**************************************************************************
  129. float idata YP=4.5,YD=115.0,YI=0.05,ERRORX;
  130. float idata XP=4.5,XD=115.0,XI=0.05,ERRORY;
  131. float idata ZP=4.0,ZD=2.0;
  132. void Angle_Calcu(void)
  133. {
  134.         Angle_ax=(GetData(ACCEL_XOUT_H))/8192;  //加速度處理
  135.         Angle_az=(GetData(ACCEL_ZOUT_H))/8192;  //加速度量程 +-4g/S
  136.         Angle_ay=(GetData(ACCEL_YOUT_H))/8192;        //轉(zhuǎn)換關(guān)系8192LSB/g

  138.         Angle_gx=(GetData(GYRO_XOUT_H)-g_x)/65.5;   //陀螺儀處理
  139.         Angle_gy=(GetData(GYRO_YOUT_H)-g_y)/65.5;   //陀螺儀量程 +-500度/S
  140.         Angle_gz=(GetData(GYRO_ZOUT_H)-g_z)/65.5;   //轉(zhuǎn)換關(guān)系65.5LSB/度
  141. //**************X軸指向*******************************************************************************
  142.         AngleAx=atan(Angle_ax/sqrt(Angle_ay*Angle_ay+Angle_az*Angle_az))*180/3.141592657;
  143.   
  144.         Anglelate=Ax;
  145.        
  146.         //Angle=0.95*(Angle-Angle_gy*dt)+0.05*AngleAx;//一階互補(bǔ)濾波       這邊是-Angle_gy
  147.        
  148.         Kalman_Filter(AngleAx,-Angle_gy);       //卡爾曼濾波
  149.        
  150.         Ax=Angle+((float)RxBuf[1]-128)/7;  
  151.         ERRORX+=3*Ax;                 //加強(qiáng)靜差積分強(qiáng)度但是不改變輸出控制量的范圍
  152.         if(ERRORX>1000){ERRORX=1000;}if(ERRORX<-1000){ERRORX=-1000;}
  153.         PID_y=Ax*XP+ERRORX*XI+(Ax-Anglelate)*XD;          
  154.         speed0=0+PID_y,speed2=0-PID_y;
  155. //**************Y軸指向*******************************************************************************
  156.   AngleAy=atan(Angle_ay/sqrt(Angle_ax*Angle_ax+Angle_az*Angle_az))*180/3.141592657;

  158.         Anglelatey=Ay;
  159.        
  160.         //Angley=0.95*(Angley+Angle_gx*dt)+0.05*AngleAy;//一階互補(bǔ)濾波     這邊是+Angle_gx
  161.        
  162.         Kalman_Filter_Y(AngleAy,Angle_gx);      //卡爾曼濾波
  163.        
  164.         Ay=Angley+((float)RxBuf[2]-128)/7-RxBuf[4]/20;           //由于結(jié)構(gòu)問題，油門越大，Y軸加速度計(jì)輸出偏差越大，RxBuf[4]補(bǔ)償之
  165.         ERRORY+=3*Ay;                 //加強(qiáng)靜差積分強(qiáng)度但是不改變輸出控制量的范圍
  166.         if(ERRORY>1000){ERRORY=1000;}if(ERRORY<-1000){ERRORY=-1000;}
  167.         PID_x=Ay*YP+ERRORY*YI+(Ay-Anglelatey)*YD;          
  168.         speed3=0+PID_x,speed1=0-PID_x;          
  169. //**************Z軸指向*******************************************************************************
  170.         Angle_gz+=(RxBuf[3]-128)/10;
  171.         PID_z=(Angle_gz)*ZP+(Angle_gz-Anglezlate)*ZD;
  172.         Anglezlate=Angle_gz;
  173.         speed0=speed0+PID_z,speed2=speed2+PID_z;
  174.         speed1=speed1-PID_z,speed3=speed3-PID_z;
  175. //*****************無線串口相關(guān)***********************************************************************
  176.         ich1=Ax;
  177.         ich2=Ay;
  178.         ich3=AngleAy;
  179.         ich4=XD;
  180. //**************速度更新******************************************************************************       
  181.         if((1000-RxBuf[4]*4+speed0)>1000)PWM0=1000;
  182.         else if((1000-RxBuf[4]*4+speed0)<0)PWM0=0;
  183.         else PWM0=(1000-RxBuf[4]*4+speed0);

  185.         if((1000-RxBuf[4]*4+speed1)>1000)PWM1=1000;
  186.         else if((1000-RxBuf[4]*4+speed1)<0)PWM1=0;
  187.         else PWM1=(1000-RxBuf[4]*4+speed1);

  189.         if((1000-RxBuf[4]*4+speed2)>1000)PWM2=1000;
  190.         else if((1000-RxBuf[4]*4+speed2)<0)PWM2=0;
  191.         else PWM2=(1000-RxBuf[4]*4+speed2);

  193.         if((1000-RxBuf[4]*4+speed3)>1000)PWM3=1000;
  194.         else if((1000-RxBuf[4]*4+speed3)<0)PWM3=0;
  195.         else PWM3=(1000-RxBuf[4]*4+speed3);
  196.   if(RxBuf[4]>=10)
  197.         {PWM(PWM1,PWM2,PWM0,PWM3);}//1203
  198.         else
  199.         {PWM(1000,1000,1000,1000);}
  200. //******************************************以下注釋內(nèi)容為PID中D的自動(dòng)整定模式********************************************
  201.         /*         
  202.         if(RxBuf[4]>60&&THEEND==0)
  203.         {
  204.                 if(Ax<0&&Ay>0)
  205.                 {
  206.                 ERRORPID[TE]=-Ax+Ay;
  207.                 }
  208.                 else if(Ax>0&&Ay<0)
  209.                 {
  210.                 ERRORPID[TE]=Ax-Ay;
  211.                 }       
  212.                 else if(Ax<0&&Ay<0)
  213.                 {
  214.                 ERRORPID[TE]=-Ax-Ay;
  215.                 }       
  216.                 else
  217.                 {
  218.                 ERRORPID[TE]=Ax+Ay;
  219.                 }
  220.                 TE++;
  221.                 if(TE==511)
  222.                 {
  223.                         TE=0;
  224.                         ZSYPID[TZA]=XD;
  225.                         XD++;YD++;
  226.                         for(ADD=0;ADD<=510;ADD++)
  227.                                 {
  228.                              ALLERROR[TZA]+=ERRORPID[ADD];
  229.                                 }
  230.                         TZA++;
  231.                         if(TZA==100)
  232.                         {
  233.                                 THEEND=1;
  234.                                 LESTERROR=ALLERROR[1];
  235.                                 for(TE=1;TE<=99;TE++)
  236.                                 {
  237.                                         if(LESTERROR>=ALLERROR[TE])
  238.                                         {
  239.                                                 LESTERROR=ALLERROR[TE];
  240.                                                 BESTPID=ZSYPID[TE];
  241.                                         }
  242.                                 }
  243.                                 XD=YD=BESTPID;
  244.                         }
  245.                 }
  246.         }       
  247.         */
  248. }
  249. void main()
  250. {
  251. Delay(1000);
  252. PWMGO();
  253. init_NRF24L01();
  254. InitMPU6050();
  255. Time0_Init();
  256. RxBuf[1]=128;
  257. RxBuf[2]=128;
  258. RxBuf[3]=128;
  259. RxBuf[4]=0;
  260.         //SetRX_Mode();
  261. while(1)
  262. {          
  263.                     //以下為無線串口助手發(fā)送程序
  264.                                 TxBuf[1]=13; TxBuf[6]=13; TxBuf[11]=15; TxBuf[16]=15;
  265.                           if(ich1<0)         {TxBuf[1]=12; ich1=-ich1;}
  266.                                 if(ich2<0)         {TxBuf[6]=12; ich2=-ich2;}
  267.                                 if(ich3<0)         {TxBuf[11]=12; ich3=-ich3;}
  268.                                 if(ich4<0)         {TxBuf[16]=12; ich4=-ich4;}
  269.                                 TxBuf[2]=ich1/1000;        TxBuf[3]=ich1/100%10; TxBuf[4]=ich1%100/10;        TxBuf[5]=ich1%10;         
  270.                                 TxBuf[7]=ich2/1000;        TxBuf[8]=ich2/100%10; TxBuf[9]=ich2%100/10;        TxBuf[10]=ich2%10;
  271.                                 TxBuf[12]=ich3/1000; TxBuf[13]=ich3/100%10;        TxBuf[14]=ich3%100/10; TxBuf[15]=ich3%10;
  272.                                 TxBuf[17]=ich4/1000; TxBuf[18]=ich4/100%10;        TxBuf[19]=ich4%100/10; TxBuf[0]=ich4%10;
  273.                                 //以上為無線串口助手發(fā)送程序
  274.                                 nRF24L01_TxPacket(TxBuf);
  275. ……………………

  277. …………限于本文篇幅 余下代碼請(qǐng)從51黑下載附件…………
復(fù)制代碼

所有資料51hei提供下載:
四軸-四元數(shù)測(cè)試版.rar (197.51 KB, 下載次數(shù): 34)


作者: 馬帥印    時(shí)間: 2024-10-17 01:16
完全看不懂




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