久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

標(biāo)題: 四軸飛行器stc單片機(jī)程序 [打印本頁(yè)]
作者: 陳好    時(shí)間: 2017-9-4 10:14
標(biāo)題: 四軸飛行器stc單片機(jī)程序
stc也可以做四軸


單片機(jī)源程序如下:
  1. //========================================================================
  2. //        作者：xiaoliu
  3. //        郵箱：1042763631
  4. //        版本V1.0
  5. //========================================================================
  6. //                                 愛(ài)好者電子工作室
  7. //特別聲明：此程序
  8. //         
  9. //
  10. //
  11. //                                 MCU工作頻率12MHz
  12. //=========================================================================
  13. //1、如果需要更改工作頻率，請(qǐng)修改本工程中的config.h頭文件中 MAIN_Fosc的宏定義，
  14. //   延時(shí)函數(shù)都會(huì)保持一致，無(wú)需更改延遲的參數(shù)。
  15. //2、波特率為2400，如果需要更改，必須和STC-ISP最低波特率保持一致才能實(shí)現(xiàn)自動(dòng)下載
  16. //3、IO口已被重新定義，IAP15W4K58S4最大封裝為64腳，具有62個(gè)IO口，其中有8個(gè)模擬口，
  17. //   當(dāng)然模擬口也可用作數(shù)字口，數(shù)字用D表示，模擬用A來(lái)表示，此定義專(zhuān)門(mén)為STC15W4K系列
  18. //   定義的IO，方便方便以后大家日后的使用，此定義方法類(lèi)似arduino。
  19. //   使用數(shù)字IO口時(shí)，定義如下：
  20. //   P3.0-P3.7--->D0-D7   也可以直接使用0-7
  21. //   P2.0-P2.7--->D8-D15  也可以直接使用8-15
  22. //   P4.0-P4.7--->D16-D23 也可以直接使用16-23
  23. //   P5.0-P5.7--->D24-D31 也可以直接使用24-31
  24. //   P6.0-P6.7--->D32-D39 也可以直接使用32-39
  25. //   P7.0-P7.7--->D40-D47 也可以直接使用40-47
  26. //   P0.0-P0.7--->D48-D55 也可以直接使用48-55
  27. //   P1.0-P1.7--->D56-D63 也可以直接使用56-63 也可以使用A0-A7
  28. //   使用模擬IO口時(shí)，定義如下：
  29. //   P1.0-P1.7--->0-7
  30. //4、pinMode、digitalWrite必須使用數(shù)字IO定義方法，analogRead必須使用模擬IO定義方法
  31. //=============================================================================
  32. #include "config.h"
  33. #include "GPIO.h"
  34. #include "usart.h"
  35. #include "delay.h"
  36. #include "epwm.h"
  37. #include "eInterrupt.h"
  38. #include "IMU.h"
  39. #include "nrf24l01.h"
  40. #include "MPU6050.h"
  41. #include "eeprom.h"
  42. #define Q15(X) \
  43.    ((X < 0.0) ? (int)(32768*(X) - 0.5) : (int)(32767*(X) + 0.5))
  44. #define        EAXSFR()                P_SW2 |=  0x80        /* MOVX A,@DPTR/MOVX @DPTR,A指令的操作對(duì)象為擴(kuò)展SFR(XSFR) */
  45. //PWM定義
  46. #define PWM_L 11 //左邊的電機(jī)
  47. #define PWM_R 7  //右邊的電機(jī)
  48. #define PWM_F 10 //前面的電機(jī)
  49. #define PWM_B 9  //后面的電機(jī)
  50. //mpu6050定義         
  51. #define SDA         22         //定義mpu6050的SDA的數(shù)字io
  52. #define SCL   48         //定義mpu6050的SCL的數(shù)字io


  55. #define CE    23         
  56. #define CSN   57
  57. #define SCK   56
  58. #define MOSI  34
  59. #define MISO  33
  60. #define IRQ   32

  62. #define LED_4   18
  63. #define LED_2   55
  64. #define LED_3   25
  65. #define LED_1   28

  67. u8 SW2_tmp;
  68. bit        B_8ms;
  69. u8 count;
  70. //******************************************************************************************************
  71. u8        xdata        TxBuf[20]={0};
  72. u8        xdata        RxBuf[20]={0};
  73. u8  xdata RxBuf_temp[20]={0};
  74. u8        xdata        tp[16];
  75. //******************************************************************************************************
  76. u8        YM=0;              //油門(mén)變化速度控制，不這樣做的話(huà)快速變化油門(mén)時(shí)四軸會(huì)失速翻轉(zhuǎn)

  78. int                data speed2=0,speed3=0,speed4=0,speed5=0;           //電機(jī)速度參數(shù)
  79. int                data PWM2=0,PWM3=0,PWM4=0,PWM5=0;        //加載至PWM模塊的參數(shù)
  80. int                xdata g_x=0,g_y=0,g_z=0;                        //陀螺儀矯正參數(shù)
  81. int                xdata a_x,a_y;                                                //角度矯正參數(shù)

  83. long  xdata g_x_aver=0;
  84. long  xdata g_y_aver=0;
  85. long  xdata g_z_aver=0;
  86. int   delta_rc_x=0;
  87. int   delta_rc_y=0;
  88. int   delta_rc_z=0;

  90. u8 YM_last;
  91. int delta_Ax;
  92. int delta_Ay;
  93. int delta_Az;

  95. int Ax_real;
  96. int Ay_real;
  97. int Az_real;

  99. int delta_Ax_last;
  100. int delta_Ay_last;
  101. int delta_Az_last;
  102. //*****************角度參數(shù)*************************************************
  103. int xdata Angle_ax=0,Angle_ay=0,Angle_az=0;        //由加速度計(jì)算的加速度(弧度制)
  104. int idata Angle_gy=0,Angle_gx=0,Angle_gz=0;        //由角速度計(jì)算的角速率(角度制)
  105. int data  AngleX=0,AngleY=0,AngleZ=0;                                //四元數(shù)解算出的歐拉角
  106. float xdata  Anglezlate=0;                                //Z軸相關(guān)
  107. float xdata IMU_gz;
  108. int xdata  Ax=0,Ay=0;                                        //加入遙控器控制量后的角度
  109. int xdata  gx=0,gy=0;                                        //加入遙控器控制量后的角?
  110. u8 data lastR0=0,ZT=0; //上一次RxBuf[0]數(shù)據(jù)(RxBuf[0]數(shù)據(jù)在不斷變動(dòng)的)   狀態(tài)標(biāo)識(shí)
  111. //****************姿態(tài)處理和PID*********************************************
  112. int        data PID_Output;        //PID最終輸出量

  114. float        xdata Last_Angle_gx=0;        //外環(huán)PI輸出量  上一次陀螺儀數(shù)據(jù)
  115. long        xdata ERRORX_Out=0;        //外環(huán)P  外環(huán)I  外環(huán)誤差積分
  116. long        xdata ERRORX_In=0;        //內(nèi)環(huán)P  內(nèi)環(huán)I   內(nèi)環(huán)D  內(nèi)環(huán)誤差積分

  118. float        xdata Last_Angle_gy=0;
  119. long        xdata ERRORY_Out=0;
  120. long        xdata ERRORY_In=0;
  121. long        xdata ERRORZ_Out=0;

  123. int xdata Last_Ax=0;
  124. int xdata Last_Ay=0;

  126. unsigned int LED_num=0;

  128. int xdata Last_gx=0;
  129. int xdata Last_gy=0;
  130. long idata PID_P;
  131. long idata PID_I;
  132. long idata PID_D;
  133. #define        Out_XP        18.0f            //外環(huán)P
  134. #define        Out_XI        Q15(0.024)        //外環(huán)I                Q15(0.024)
  135. #define        Out_XD        3.0f            //外環(huán)D

  137. #define        In_XP        Q15(0.3) //內(nèi)環(huán)P 720        0.3                                0.3                 0.5
  138. #define        In_XI        Q15(0.00)//內(nèi)環(huán)I                0.024                   0                  0
  139. #define        In_XD        7.5f                 //內(nèi)環(huán)D 720   5                  7.5                   8.0


  142. #define        In_YP        In_XP
  143. #define        In_YI        In_XI
  144. #define        In_YD        In_XD

  146. #define        Out_YP        Out_XP
  147. #define        Out_YI        Out_XI
  148. #define        Out_YD        Out_XD

  150. #define        ZP        5.0f                          //  5
  151. #define        ZD        8.0f        //自旋控制的PID                 8
  152. #define ZI  Q15(0.24)                        //  0.24

  154. #define        ERR_MAX        8000
  155. #define KALMAN_Q        Q15(0.20)
  156. #define KALMAN_R        Q15(0.80)
  157.          
  158.          
  159. int KalmanFilter_ax( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R);
  160. int KalmanFilter_ay( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R);
  161. int KalmanFilter_az( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R);
  162. int KalmanFilter_gyrox( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R);
  163. int KalmanFilter_gyroy( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R);
  164. int KalmanFilter_gyroz( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R);       
  165. void main()
  166. {               
  167.                 int i=1;
  168.                 u8 j;          
  169.                 Serialbegin(2400,1,COM1);//設(shè)置COM1的波特率為2400，將“最低波特率”的值設(shè)為2400即可實(shí)現(xiàn)自動(dòng)下載
  170.                 pinMode(SDA,pullup);//設(shè)置為內(nèi)部上拉模式
  171.                 pinMode(SCL,pullup);//設(shè)置為內(nèi)部上拉模式
  172.        
  173.                 pinMode(PWM_L,OUTPP);//設(shè)置成推挽模式
  174.                 pinMode(PWM_R,OUTPP);//設(shè)置成推挽模式
  175.                 pinMode(PWM_F,OUTPP);//設(shè)置成推挽模式
  176.                 pinMode(PWM_B,OUTPP);//設(shè)置成推挽模式

  178.                 pinMode(LED_4,OUTPP);//設(shè)置成推挽模式       
  179.                 pinMode(LED_2,OUTPP);//設(shè)置成推挽模式                
  180.                 pinMode(LED_3,OUTPP);//設(shè)置成推挽模式
  181.                 pinMode(LED_1,OUTPP);//設(shè)置成推挽模式         

  183.                 pinMode(CE,pullup);//設(shè)置為內(nèi)部上拉模式
  184.                 pinMode(CSN,pullup);//設(shè)置為內(nèi)部上拉模式
  185.                 pinMode(SCK,pullup);//設(shè)置為內(nèi)部上拉模式
  186.                 pinMode(MOSI,pullup);//設(shè)置為內(nèi)部上拉模式
  187.                 pinMode(MISO,pullup);//設(shè)置為內(nèi)部上拉模式
  188.                 pinMode(IRQ,pullup);//設(shè)置為內(nèi)部上拉模式
  189.                
  190.                 //以下為PWM初始化       
  191.               epwmnTcolck(12);//系統(tǒng)時(shí)鐘，不分頻
  192.                 epwmTwide(20000);//設(shè)置PWM寬度為1001
  193.                 epwmStartADC(0);//不觸發(fā)AD
  194.                 epwmFirstSignalLevel(PWM_L,0);//設(shè)置初始電平為低
  195.                 epwmFirstSignalLevel(PWM_R,0);//設(shè)置初始電平為低
  196.                 epwmFirstSignalLevel(PWM_F,0);//設(shè)置初始電平為低
  197.                 epwmFirstSignalLevel(PWM_B,0);//設(shè)置初始電平為低
  198.                
  199.                 epwmSetValue(PWM_L,19300,20000);//設(shè)置翻轉(zhuǎn)值，第1次翻轉(zhuǎn)為1000，第2次翻轉(zhuǎn)為1001
  200.                 epwmSetValue(PWM_R,19300,20000);//設(shè)置翻轉(zhuǎn)值，第1次翻轉(zhuǎn)為1000，第2次翻轉(zhuǎn)為1001
  201.                 epwmSetValue(PWM_F,19300,20000);//設(shè)置翻轉(zhuǎn)值，第1次翻轉(zhuǎn)為1000，第2次翻轉(zhuǎn)為1001
  202.                 epwmSetValue(PWM_B,19300,20000);//設(shè)置翻轉(zhuǎn)值，第1次翻轉(zhuǎn)為1000，第2次翻轉(zhuǎn)為1001
  203.                 epwmCRbegin();//啟動(dòng)PWM計(jì)數(shù)器
  204.                 init_NRF24L01();//初始化無(wú)線(xiàn)模塊
  205.                 RxBuf[1] = 128;
  206.                 RxBuf[2] = 128;
  207.                 RxBuf[3] = 128;
  208.                 RxBuf[4] = 0;
  209.                 IAPRead();                //讀取陀螺儀靜差
  210.                 InitMPU6050();        //初始化MPU-6050
  211.                 Timer0_attachInterrupt(10000);//定時(shí)器0以10ms定時(shí)中斷
  212.                 Timer3_attachInterrupt(4000);//
  213.                   loop()
  214.                 {                                                         
  215.                         YM = RxBuf[4];        //油門(mén)

  217.                             
  218.                        
  219.                         if((RxBuf[5] == 1)||(RxBuf[6] == 1))        //校準(zhǔn)時(shí)，請(qǐng)按2次，第1次清除角速度，第2次清除角度
  220.                         {
  221.                                 RxBuf[5] = 0;
  222.                                 RxBuf[6] = 0;
  223.                                 RxBuf_temp[5] = 0;
  224.                                 RxBuf_temp[6] = 0;
  225.                                 if(RxBuf[4] < 20)
  226.                                 {               
  227.                                
  228.                                
  229.                                         EA = 0;
  230.                                         TR0=0;
  231.                                         g_x_aver=0;
  232.                                         g_y_aver=0;
  233.                                         g_z_aver=0;
  234.                                         for(j=0;j<64;j++)
  235.                                         {               
  236.                                                         Read_MPU6050(tp);
  237.                                                         g_x_aver=g_x_aver+(((int *)&tp)[4]);
  238.                                                         g_y_aver=g_y_aver+(((int *)&tp)[5]);
  239.                                                         g_z_aver=g_z_aver+(((int *)&tp)[6]);                                               
  240.                                         }               
  241.                                         g_x_aver=g_x_aver>>6;
  242.                                         g_y_aver=g_y_aver>>6;
  243.                                         g_z_aver=g_z_aver>>6;
  244.                                        
  245.                                         IAP_Gyro();
  246.    
  247.                                         epwmSetValue(PWM_L,19300,20000);//設(shè)置翻轉(zhuǎn)值，第1次翻轉(zhuǎn)為960，第2次翻轉(zhuǎn)為1001
  248.                                         epwmSetValue(PWM_R,19300,20000);//設(shè)置翻轉(zhuǎn)值，第1次翻轉(zhuǎn)為960，第2次翻轉(zhuǎn)為1001
  249.                                         epwmSetValue(PWM_F,19300,20000);//設(shè)置翻轉(zhuǎn)值，第1次翻轉(zhuǎn)為960，第2次翻轉(zhuǎn)為1001
  250.                                         epwmSetValue(PWM_B,19300,20000);//設(shè)置翻轉(zhuǎn)值，第1次翻轉(zhuǎn)為960，第2次翻轉(zhuǎn)為1001
  251.                                         delay_ms(100); //校準(zhǔn)完畢滴一聲

  253.                                         epwmSetValue(PWM_L,19300,20000);//設(shè)置翻轉(zhuǎn)值，第1次翻轉(zhuǎn)為1000，第2次翻轉(zhuǎn)為1001
  254.                                         epwmSetValue(PWM_R,19300,20000);//設(shè)置翻轉(zhuǎn)值，第1次翻轉(zhuǎn)為1000，第2次翻轉(zhuǎn)為1001
  255.                                         epwmSetValue(PWM_F,19300,20000);//設(shè)置翻轉(zhuǎn)值，第1次翻轉(zhuǎn)為1000，第2次翻轉(zhuǎn)為1001
  256.                                         epwmSetValue(PWM_B,19300,20000);//設(shè)置翻轉(zhuǎn)值，第1次翻轉(zhuǎn)為1000，第2次翻轉(zhuǎn)為1001
  257.                                         TR0=1;
  258.                                         EA = 1;
  259.                                 }
  260.                         }

  262.                 }
  263. }

  265. void timer0_int (void) interrupt TIMER0_VECTOR
  266. {
  267.         B_8ms = 1;
  268.         P52=1;

  270.          LED_num++;
  271.         if(LED_num>200)
  272.         {
  273.            LED_num=0;
  274. //           P51=1;                               
  275. //           P42=1;
  276. //           P07=1;
  277. //           P54=1;                  
  278.         }
  279.         if(LED_num==50)
  280.         {
  281. //           P51=0;                       
  282. //           P42=0;
  283. //           P07=0;
  284. //           P54=0;                  
  285.         }
  286.        
  287.         if(RxBuf[0] == lastR0)        //如果RxBuf[0]的數(shù)據(jù)沒(méi)有收到 即失聯(lián)
  288.         {
  289.                 if(++ZT >= 128)
  290.                 {
  291.                         ZT = 120;        //狀態(tài)標(biāo)識(shí)大于128即1秒沒(méi)有收到數(shù)據(jù)，失控保護(hù)
  292.                        
  293.                         RxBuf[1] = 128;
  294.                         RxBuf_temp[1] = 128;
  295.                         RxBuf[2] = 128; //觸發(fā)失控保護(hù) ，緩慢下降，俯仰橫滾方向舵歸中
  296.                         RxBuf_temp[2]=128;
  297.                         RxBuf[3] = 128;
  298.                         RxBuf_temp[3]=128;
  299.                         if(RxBuf[4] != 0)       
  300.                         {
  301.                                 RxBuf[4]--;        //油門(mén)在原值逐漸減小
  302.                                 RxBuf_temp[4]=RxBuf[4];
  303.                         }
  304.                 }
  305.         }
  306.         else        ZT = 0;
  307.         lastR0 = RxBuf[0];

  309.         Read_MPU6050(tp);
  310.     Angle_ax=KalmanFilter_ax( ((int *)&tp)[0],KALMAN_Q,KALMAN_R);
  311.         Angle_ay=KalmanFilter_ay( ((int *)&tp)[1],KALMAN_Q,KALMAN_R);
  312.         Angle_az=KalmanFilter_az( ((int *)&tp)[2],KALMAN_Q,KALMAN_R);

  314.         Angle_gx = ((float)(((int *)&tp)[4] - g_x)) / 65.5;        //陀螺儀處理        結(jié)果單位是 +-度
  315.         Angle_gy = ((float)(((int *)&tp)[5] - g_y)) / 65.5;        //陀螺儀量程 +-500度/S, 1度/秒 對(duì)應(yīng)讀數(shù) 65.536
  316.         Angle_gz=KalmanFilter_gyroz( ((int *)&tp)[6],Q15(0.2),Q15(0.8))- g_z;
  317.         IMU_gz=Angle_gz/65.5;
  318.         Last_Angle_gx = Angle_gx;                //儲(chǔ)存上一次角速度數(shù)據(jù)
  319.         Last_Angle_gy = Angle_gy;


  322.        
  323. //*********************************** 四元數(shù)解算 ***********************************
  324.         IMUupdate(Angle_gx*0.0174533f, Angle_gy*0.0174533f, IMU_gz*0.0174533f, Angle_ax,Angle_ay,Angle_az);//姿態(tài)解算，精度0.1度
  325. //將四軸飛行器的所有數(shù)據(jù)發(fā)到遙控上       
  326.         TxBuf[0]=AngleX-a_x;
  327.         TxBuf[1]=(AngleX-a_x)>>8;
  328.         TxBuf[2]=Angle_ax;
  329.         TxBuf[3]=Angle_ax>>8;
  330.         TxBuf[4]=((int *)&tp)[4]-g_x;
  331.         TxBuf[5]=(((int *)&tp)[4]-g_x)>>8;
  332.         TxBuf[6]=AngleY-a_y;
  333.         TxBuf[7]=(AngleY-a_y)>>8;       
  334.         TxBuf[8]=Angle_ay;
  335.         TxBuf[9]=Angle_ay>>8;       
  336.         TxBuf[10]=((int *)&tp)[5]-g_y;
  337.         TxBuf[11]=(((int *)&tp)[5]-g_y)>>8;       
  338.         TxBuf[12]=AngleZ;//z軸角度始終為0，因?yàn)椴荒軓腎MU中獲取
  339.         TxBuf[13]=(AngleZ)>>8;
  340.         TxBuf[14]=Angle_az;
  341.         TxBuf[15]=Angle_az>>8;
  342.         TxBuf[16]=Angle_gz;
  343.         TxBuf[17]=Angle_gz>>8;
  344.         //0.174533為PI/180 目的是將角度轉(zhuǎn)弧度        四元數(shù)計(jì)算出 俯仰(X軸): AngleX, 橫滾(Y軸): AngleY                       
  345. //************** MPU6050 Y軸與水平傾角 ***********************************************************
  346.         if((RxBuf[2] > 113) && (RxBuf[2] < 143))        RxBuf[2] = 128;                 //113 143 128
  347.         delta_rc_y=128-RxBuf[2] ;
  348.         delta_rc_y=delta_rc_y*10/7;
  349.         //        Ay為誤差，delta_rc_y為遙控給的角度作為基準(zhǔn)，RxBuf[8]為微調(diào)量，a_y為偏差角度，AngleY為采樣的角度
  350.         //一般的，在不考慮微調(diào)、偏移量，誤差=基準(zhǔn)-測(cè)量值
  351.         Ay  = delta_rc_y -(char)RxBuf[8]*2+a_y-AngleY;
  352.         if(YM > 20)                ERRORY_Out += Ay;                                //外環(huán)積分(油門(mén)小于某個(gè)值時(shí)不積分)
  353.         else                        ERRORY_Out = 0;                                        //油門(mén)小于定值時(shí)清除積分值
  354.         if(ERRORY_Out >  ERR_MAX)        ERRORY_Out =  ERR_MAX;
  355.         else if(ERRORY_Out < -ERR_MAX)        ERRORY_Out = -ERR_MAX;        //積分限幅
  356.         PID_P=(long)Ay*Out_YP;
  357.         PID_I=((long)ERRORY_Out*Out_YI)>>15;
  358.         PID_D=((Angle_gx+Last_Angle_gx)/2)*Out_YD;
  359.         PID_Output = (PID_P +PID_I+PID_D+5)/10;        //外環(huán)PID，“+5”為了四舍五入?
  360.         Last_Ay=Ay;
  361.        
  362.         gy=PID_Output - Angle_gx;
  363.         if(YM > 20)ERRORY_In +=gy;//內(nèi)環(huán)積分(油門(mén)小于某個(gè)值時(shí)不積分)
  364.         else                        ERRORY_In = 0;                                                        //油門(mén)小于定值時(shí)清除積分值
  365.         if(ERRORY_In >  ERR_MAX)        ERRORY_In =  ERR_MAX;
  366.         else if(ERRORY_In < -ERR_MAX)        ERRORY_In = -ERR_MAX;        //積分限幅
  367.         PID_P=((long)gy*In_YP)>>15;
  368.         PID_I=((long)ERRORY_In*In_YI)>>15;
  369.         PID_D=((long)gy - Last_gy)*In_YD;
  370.         PID_Output =PID_P+PID_I+PID_D;
  371.         Last_gy=gy;
  372.         if(PID_Output >  19300)        PID_Output =  19300;  //輸出量限幅
  373.         if(PID_Output < -19300)        PID_Output = -19300;
  374.         speed5 =  PID_Output,        speed2 =   -PID_Output;//加載到速度參數(shù)
  375. //**************MPU6050 X軸與水平傾角**************************************************
  376.         if((RxBuf[1] > 113) && (RxBuf[1] < 143))        RxBuf[1] = 128;       
  377.         delta_rc_x=RxBuf[1] - 128;
  378.         delta_rc_x=delta_rc_x*10/7;
  379.         //        Ax為誤差，delta_rc_x為遙控給的角度作為基準(zhǔn)，RxBuf[7]為微調(diào)量，a_x為偏差角度，AngleX為采樣的角度
  380.         //一般的，在不考慮微調(diào)、偏移量，誤差=基準(zhǔn)-測(cè)量值
  381.         Ax = delta_rc_x+(char)RxBuf[7]*2+a_x-AngleX;
  382.         if(YM > 20)        ERRORX_Out += Ax;        //外環(huán)積分(油門(mén)小于某個(gè)值時(shí)不積分)
  383.         else                ERRORX_Out = 0;                //油門(mén)小于定值時(shí)清除積分值
  384.         if(ERRORX_Out >  ERR_MAX)        ERRORX_Out =  ERR_MAX;        //積分限幅
  385.         else if(ERRORX_Out < -ERR_MAX)        ERRORX_Out = -ERR_MAX;        //積分限幅       
  386.         PID_P=(long)Ax*Out_YP;
  387.         PID_I=((long)ERRORX_Out*Out_XI)>>15;
  388.         PID_D=((Angle_gy+Last_Angle_gy)/2)*Out_XD;
  389.         PID_Output = (PID_P+PID_I+PID_D+5)/10;        //外環(huán)PID
  390.         Last_Ax=Ax;
  391.         gx=PID_Output - Angle_gy;
  392.         if(YM > 20)        ERRORX_In += gx;        //內(nèi)環(huán)積分(油門(mén)小于某個(gè)值時(shí)不積分)
  393.         else                ERRORX_In  = 0; //油門(mén)小于定值時(shí)清除積分值
  394.         if(ERRORX_In >  ERR_MAX)        ERRORX_In =  ERR_MAX;
  395.         else if(ERRORX_In < -ERR_MAX)        ERRORX_In = -ERR_MAX;        //積分限幅

  397.         PID_P=((long)gx*In_XP)>>15;
  398.         PID_I=((long)ERRORX_In*In_XI)>>15;
  399.         PID_D=((long)gx - Last_gx)*In_XD;
  400.         PID_Output =PID_P+PID_I+PID_D;
  401.        
  402.         Last_gx=gx;
  403.         if(PID_Output >  19300)        PID_Output =  19300;  //輸出量限幅
  404.         if(PID_Output < -19300)        PID_Output = -19300;
  405.                        
  406.                                                                                 speed4 = 0 - PID_Output;
  407.        
  408.                                                                                 speed3 = 0 + PID_Output;
  409.        
  410. //************** MPU6050 Z軸指向 *****************************       
  411. //只做速度環(huán)即可
  412.         if((RxBuf[3] > 113) && (RxBuf[3] < 143))        RxBuf[3] = 128;
  413.         delta_rc_z = (RxBuf[3] - 128)-Angle_gz;        //操作量       
  414.         delta_rc_z = ((int)RxBuf[3] - 128)*64-Angle_gz;        //本來(lái)是乘與65.5的系數(shù)，乘與64也可以
  415.         if(YM > 20)                ERRORZ_Out += delta_rc_z;
  416.         else                        ERRORZ_Out  = 0;
  417.         if(ERRORZ_Out >  50000)        ERRORZ_Out =  50000;
  418.         else if(ERRORZ_Out < -50000)        ERRORZ_Out = -50000;        //積分限幅
  419.         PID_P=((long)delta_rc_z)*ZP;
  420.         PID_I=((long)ERRORZ_Out * ZI)>>15;
  421.         PID_D=((long)delta_rc_z - Anglezlate) * ZD;

  423.         PID_Output =( PID_P + PID_I + PID_D)>>6;                                                                                 
  424.        
  425.         Anglezlate = delta_rc_z;
  426.         if(PID_Output >  19300)        PID_Output =  19300;  //輸出量限幅
  427.         if(PID_Output < -19300)        PID_Output = -19300;
  428.                 speed4 = speed4 + PID_Output;
  429.                 speed5 = speed5 - PID_Output;speed2 = speed2 - PID_Output;
  430.                 speed3 = speed3 + PID_Output;
  431.                
  432. //**************將速度參數(shù)加載至PWM模塊*************************************************       
  433.         if(YM < 20)                PWM2 = 19300,        PWM3 = 19300,        PWM4 = 19300,        PWM5 = 19300;
  434.         else
  435.         {
  436.                 PWM2 = 19300 - (int)YM*4 - speed2;
  437.                          if(PWM2 > 19300)        PWM2 = 19300;        //速度參數(shù)控制，防止超過(guò)PWM參數(shù)范圍0-1000
  438.                 else if(PWM2 < 0)                PWM2 = 0;

  440.                 PWM3 = 19300 - (int)YM*4 - speed3;
  441.                          if(PWM3 > 19300)        PWM3 = 19300;
  442.                 else if(PWM3 < 0)                PWM3 = 0;

  444.                 PWM4 = 19300 - (int)YM*4 - speed4;
  445.                          if(PWM4 > 19300)        PWM4 = 19300;
  446.                 else if(PWM4 < 0)                PWM4 = 0;

  448.                 PWM5 = 19300 - (int)YM*4 - speed5;
  449.                          if(PWM5 > 19300)        PWM5 = 19300;
  450.                 else if(PWM5 < 0)                PWM5 = 0;
  451.         }
  452.         epwmSetValue(PWM_L,PWM5,20000);//設(shè)置翻轉(zhuǎn)值，第1次翻轉(zhuǎn)為PWM5，第2次翻轉(zhuǎn)為1001
  453.         epwmSetValue(PWM_R,PWM2,20000);//設(shè)置翻轉(zhuǎn)值，第1次翻轉(zhuǎn)為PWM2，第2次翻轉(zhuǎn)為1001
  454.         epwmSetValue(PWM_F,PWM4,20000);//設(shè)置翻轉(zhuǎn)值，第1次翻轉(zhuǎn)為PWM4，第2次翻轉(zhuǎn)為1001
  455.         epwmSetValue(PWM_B,PWM3,20000);//設(shè)置翻轉(zhuǎn)值，第1次翻轉(zhuǎn)為PWM3，第2次翻轉(zhuǎn)為1001
  456.         P52=0;
  457.         }
  458. void timer3_int (void) interrupt 19
  459. {
  460.                  count++;
  461.                         if(count==1)
  462.                         {
  463.                                 nRF24L01_TxPacket(TxBuf);//發(fā)射數(shù)據(jù)
  464.                         }
  465.                         else if(count==3)
  466.                         {
  467.                                 SetRX_Mode();
  468.                         }
  469.                         else if(count==4)
  470.                         {                               
  471.                                 nRF24L01_RxPacket(RxBuf_temp);//接收數(shù)據(jù)
  472.                                 if((RxBuf_temp[9]-RxBuf_temp[8]-RxBuf_temp[7]-RxBuf_temp[6]-RxBuf_temp[5]-RxBuf_temp[4]-RxBuf_temp[3]-RxBuf_temp[2]-RxBuf_temp[1]-RxBuf_temp[0])==0)
  473.                                 {                                       
  474.                                         RxBuf[8]=RxBuf_temp[8];//左右微調(diào)
  475.                                         RxBuf[7]=RxBuf_temp[7];//前后微調(diào)
  476.                                         RxBuf[6]=RxBuf_temp[6];
  477.                                         RxBuf[5]=RxBuf_temp[5];
  478.                                         RxBuf[4]=RxBuf_temp[4];
  479.                                         RxBuf[3]=RxBuf_temp[3];
  480.                                         RxBuf[2]=RxBuf_temp[2];
  481.                                         RxBuf[1]=RxBuf_temp[1];
  482.                                         RxBuf[0]=RxBuf_temp[0];
  483.                                 }
  484.                                 count=0;
  485.                         }
  486. }
  487. int KalmanFilter_ax( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R)
  488. {
  489.    int R = MeasureNoise_R;
  490.    int Q = ProcessNiose_Q;
  491.    static int ax_last;
  492.    int ax_mid = ax_last;
  493.    long ax_now;
  494.    static int ax_p_last;
  495.    long p_mid ;
  496.    long p_now;
  497.    int kg;        
  498.          long temp;
  499.    ax_mid=ax_last;
  500.    p_mid=ax_p_last+Q;
  501.          temp=p_mid<<15;
  502.          kg=(temp/((long)p_mid+R));
  503.    ax_now= ax_mid+(((long)kg*(ResrcData-ax_mid))>>15);
  504.    p_now=((long)p_mid*(32768-kg))>>15;      
  505.    ax_p_last = p_now;
  506.    ax_last = ax_now;
  507.    return ax_now;               
  508. }
  509. int KalmanFilter_ay( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R)
  510. {
  511.    int R = MeasureNoise_R;
  512.    int Q = ProcessNiose_Q;
  513.    static int ay_last;
  514.    int ay_mid = ay_last;
  515.    long ay_now;
  516.    static int ay_p_last;
  517.    long p_mid ;
  518.    long p_now;
  519.    int kg;        
  520.          long temp;
  521.    ay_mid=ay_last;
  522.    p_mid=ay_p_last+Q;
  523.    temp=p_mid<<15;
  524.         kg=(temp/((long)p_mid+R));

  526.    ay_now= ay_mid+(((long)kg*(ResrcData-ay_mid))>>15);
  527.    p_now=((long)p_mid*(32768-kg))>>15;      
  528.    ay_p_last = p_now;
  529.    ay_last = ay_now;
  530.    return ay_now;               
  531. }
  532. int KalmanFilter_az( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R)
  533. {
  534.    int R = MeasureNoise_R;
  535.    int Q = ProcessNiose_Q;
  536.    static int az_last;
  537.    int az_mid = az_last;
  538.    long az_now;
  539.    static int az_p_last;
  540.    long p_mid ;
  541.    long p_now;
  542.    int kg;        
  543.         long temp;
  544.    az_mid=az_last;
  545.    p_mid=az_p_last+Q;
  546.         temp=p_mid<<15;
  547.         kg=(temp/((long)p_mid+R));

  549.    az_now= az_mid+(((long)kg*(ResrcData-az_mid))>>15);
  550.    p_now=((long)p_mid*(32768-kg))>>15;      
  551.    az_p_last = p_now;
  552.    az_last = az_now;
  553.    return az_now;               
  554. }
  555. int KalmanFilter_gyrox( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R)
  556. {
  557.    int R = MeasureNoise_R;
  558. ……………………

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

所有資料51hei提供下載:
黑色板子四軸程序.zip (555.97 KB, 下載次數(shù): 80)



作者: 41886036    時(shí)間: 2017-9-15 15:30
謝謝謝謝樓主
作者: 41886036    時(shí)間: 2017-9-15 15:48
黑幣不夠下載了
作者: 41886036    時(shí)間: 2017-9-15 15:48
黑幣不夠下載了



歡迎光臨 (http://m.zg4o1577.cn/bbs/) Powered by Discuz! X3.1
主站蜘蛛池模板： 一级黄色毛片免费 | 精产国产伦理一二三区 | 麻豆av一区二区三区久久 | 国产精品久久久亚洲 | 精品国产乱码久久久久久中文 | 美女日批免费视频 | www国产亚洲精品 | 欧美一二三区 | 自拍视频网站 | 欧美精品在线播放 | 在线视频国产一区 | 久久久久久99 | 精品久久影院 | 99re66在线观看精品热 | 久久久久久久久久久蜜桃 | 美女黄色在线观看 | 国产精品久久久久久久久污网站 | 欧美三级电影在线播放 | 成人免费观看视频 | 中文字幕亚洲视频 | a级片www | 中文字幕 欧美 日韩 | 国产精品美女久久久久久免费 | 国产不卡一区 | 福利网址| 亚洲国产一 | 国产成人免费在线 | 91精品导航 | 日本粉嫩一区二区三区视频 | 欧美一区免费 | 美女拍拍拍网站 | 韩日一区二区 | 国产免费一区 | 瑟瑟免费视频 | 亚洲 精品 综合 精品 自拍 | 日韩欧美国产精品综合嫩v 一区中文字幕 | 97caoporn国产免费人人 | 日韩精品极品视频在线观看免费 | 超碰国产在线 | 天天综合网7799精品 | 国产一区二区三区精品久久久 |