亚洲春色中文字幕久久久-三上亚,一吻二脱三床四吻胸,国产真实伦对白视频全集,在线毛片观看,精品成品入口黄网,国产毛aⅴ片久久久,亚洲AV色香蕉一区二区三区老师,萧皇后A级艳片,色情日本视频更新,99久久亚洲精品日本无码

標題: 四軸飛行器stc單片機程序 [打印本頁]

---

作者: 陳好    時間: 2017-9-4 10:14
標題: 四軸飛行器stc單片機程序
stc也可以做四軸


單片機源程序如下:

1. //========================================================================
   
2. //        作者：xiaoliu
   
3. //        郵箱：1042763631
   
4. //        版本V1.0
   
5. //========================================================================
   
6. //                                 愛好者電子工作室
   
7. //特別聲明：此程序
   
8. //         
   
9. //
   
10. //
    
11. //                                 MCU工作頻率12MHz
    
12. //=========================================================================
    
13. //1、如果需要更改工作頻率，請修改本工程中的config.h頭文件中 MAIN_Fosc的宏定義，
    
14. //   延時函數都會保持一致，無需更改延遲的參數。
    
15. //2、波特率為2400，如果需要更改，必須和STC-ISP最低波特率保持一致才能實現自動下載
    
16. //3、IO口已被重新定義，IAP15W4K58S4最大封裝為64腳，具有62個IO口，其中有8個模擬口，
    
17. //   當然模擬口也可用作數字口，數字用D表示，模擬用A來表示，此定義專門為STC15W4K系列
    
18. //   定義的IO，方便方便以后大家日后的使用，此定義方法類似arduino。
    
19. //   使用數字IO口時，定義如下：
    
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口時，定義如下：
    
29. //   P1.0-P1.7--->0-7
    
30. //4、pinMode、digitalWrite必須使用數字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指令的操作對象為擴展SFR(XSFR) */
    
45. //PWM定義
    
46. #define PWM_L 11 //左邊的電機
    
47. #define PWM_R 7  //右邊的電機
    
48. #define PWM_F 10 //前面的電機
    
49. #define PWM_B 9  //后面的電機
    
50. //mpu6050定義         
    
51. #define SDA         22         //定義mpu6050的SDA的數字io
    
52. #define SCL   48         //定義mpu6050的SCL的數字io
    
53. 
    
54. 
    
55. #define CE    23         
    
56. #define CSN   57
    
57. #define SCK   56
    
58. #define MOSI  34
    
59. #define MISO  33
    
60. #define IRQ   32
    
61. 
    
62. #define LED_4   18
    
63. #define LED_2   55
    
64. #define LED_3   25
    
65. #define LED_1   28
    
66. 
    
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;              //油門變化速度控制，不這樣做的話快速變化油門時四軸會失速翻轉
    
77. 
    
78. int                data speed2=0,speed3=0,speed4=0,speed5=0;           //電機速度參數
    
79. int                data PWM2=0,PWM3=0,PWM4=0,PWM5=0;        //加載至PWM模塊的參數
    
80. int                xdata g_x=0,g_y=0,g_z=0;                        //陀螺儀矯正參數
    
81. int                xdata a_x,a_y;                                                //角度矯正參數
    
82. 
    
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;
    
89. 
    
90. u8 YM_last;
    
91. int delta_Ax;
    
92. int delta_Ay;
    
93. int delta_Az;
    
94. 
    
95. int Ax_real;
    
96. int Ay_real;
    
97. int Az_real;
    
98. 
    
99. int delta_Ax_last;
    
100. int delta_Ay_last;
     
101. int delta_Az_last;
     
102. //*****************角度參數*************************************************
     
103. int xdata Angle_ax=0,Angle_ay=0,Angle_az=0;        //由加速度計算的加速度(弧度制)
     
104. int idata Angle_gy=0,Angle_gx=0,Angle_gz=0;        //由角速度計算的角速率(角度制)
     
105. int data  AngleX=0,AngleY=0,AngleZ=0;                                //四元數解算出的歐拉角
     
106. float xdata  Anglezlate=0;                                //Z軸相關
     
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]數據(RxBuf[0]數據在不斷變動的)   狀態標識
     
111. //****************姿態處理和PID*********************************************
     
112. int        data PID_Output;        //PID最終輸出量
     
113. 
     
114. float        xdata Last_Angle_gx=0;        //外環PI輸出量  上一次陀螺儀數據
     
115. long        xdata ERRORX_Out=0;        //外環P  外環I  外環誤差積分
     
116. long        xdata ERRORX_In=0;        //內環P  內環I   內環D  內環誤差積分
     
117. 
     
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;
     
122. 
     
123. int xdata Last_Ax=0;
     
124. int xdata Last_Ay=0;
     
125. 
     
126. unsigned int LED_num=0;
     
127. 
     
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            //外環P
     
134. #define        Out_XI        Q15(0.024)        //外環I                Q15(0.024)
     
135. #define        Out_XD        3.0f            //外環D
     
136. 
     
137. #define        In_XP        Q15(0.3) //內環P 720        0.3                                0.3                 0.5
     
138. #define        In_XI        Q15(0.00)//內環I                0.024                   0                  0
     
139. #define        In_XD        7.5f                 //內環D 720   5                  7.5                   8.0
     
140. 
     
141. 
     
142. #define        In_YP        In_XP
     
143. #define        In_YI        In_XI
     
144. #define        In_YD        In_XD
     
145. 
     
146. #define        Out_YP        Out_XP
     
147. #define        Out_YI        Out_XI
     
148. #define        Out_YD        Out_XD
     
149. 
     
150. #define        ZP        5.0f                          //  5
     
151. #define        ZD        8.0f        //自旋控制的PID                 8
     
152. #define ZI  Q15(0.24)                        //  0.24
     
153. 
     
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);//設置COM1的波特率為2400，將“最低波特率”的值設為2400即可實現自動下載
     
170.                 pinMode(SDA,pullup);//設置為內部上拉模式
     
171.                 pinMode(SCL,pullup);//設置為內部上拉模式
     
172.        
     
173.                 pinMode(PWM_L,OUTPP);//設置成推挽模式
     
174.                 pinMode(PWM_R,OUTPP);//設置成推挽模式
     
175.                 pinMode(PWM_F,OUTPP);//設置成推挽模式
     
176.                 pinMode(PWM_B,OUTPP);//設置成推挽模式
     
177. 
     
178.                 pinMode(LED_4,OUTPP);//設置成推挽模式       
     
179.                 pinMode(LED_2,OUTPP);//設置成推挽模式                
     
180.                 pinMode(LED_3,OUTPP);//設置成推挽模式
     
181.                 pinMode(LED_1,OUTPP);//設置成推挽模式         
     
182. 
     
183.                 pinMode(CE,pullup);//設置為內部上拉模式
     
184.                 pinMode(CSN,pullup);//設置為內部上拉模式
     
185.                 pinMode(SCK,pullup);//設置為內部上拉模式
     
186.                 pinMode(MOSI,pullup);//設置為內部上拉模式
     
187.                 pinMode(MISO,pullup);//設置為內部上拉模式
     
188.                 pinMode(IRQ,pullup);//設置為內部上拉模式
     
189.                
     
190.                 //以下為PWM初始化       
     
191.               epwmnTcolck(12);//系統時鐘，不分頻
     
192.                 epwmTwide(20000);//設置PWM寬度為1001
     
193.                 epwmStartADC(0);//不觸發AD
     
194.                 epwmFirstSignalLevel(PWM_L,0);//設置初始電平為低
     
195.                 epwmFirstSignalLevel(PWM_R,0);//設置初始電平為低
     
196.                 epwmFirstSignalLevel(PWM_F,0);//設置初始電平為低
     
197.                 epwmFirstSignalLevel(PWM_B,0);//設置初始電平為低
     
198.                
     
199.                 epwmSetValue(PWM_L,19300,20000);//設置翻轉值，第1次翻轉為1000，第2次翻轉為1001
     
200.                 epwmSetValue(PWM_R,19300,20000);//設置翻轉值，第1次翻轉為1000，第2次翻轉為1001
     
201.                 epwmSetValue(PWM_F,19300,20000);//設置翻轉值，第1次翻轉為1000，第2次翻轉為1001
     
202.                 epwmSetValue(PWM_B,19300,20000);//設置翻轉值，第1次翻轉為1000，第2次翻轉為1001
     
203.                 epwmCRbegin();//啟動PWM計數器
     
204.                 init_NRF24L01();//初始化無線模塊
     
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);//定時器0以10ms定時中斷
     
212.                 Timer3_attachInterrupt(4000);//
     
213.                   loop()
     
214.                 {                                                         
     
215.                         YM = RxBuf[4];        //油門
     
216. 
     
217.                             
     
218.                        
     
219.                         if((RxBuf[5] == 1)||(RxBuf[6] == 1))        //校準時，請按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);//設置翻轉值，第1次翻轉為960，第2次翻轉為1001
     
248.                                         epwmSetValue(PWM_R,19300,20000);//設置翻轉值，第1次翻轉為960，第2次翻轉為1001
     
249.                                         epwmSetValue(PWM_F,19300,20000);//設置翻轉值，第1次翻轉為960，第2次翻轉為1001
     
250.                                         epwmSetValue(PWM_B,19300,20000);//設置翻轉值，第1次翻轉為960，第2次翻轉為1001
     
251.                                         delay_ms(100); //校準完畢滴一聲
     
252. 
     
253.                                         epwmSetValue(PWM_L,19300,20000);//設置翻轉值，第1次翻轉為1000，第2次翻轉為1001
     
254.                                         epwmSetValue(PWM_R,19300,20000);//設置翻轉值，第1次翻轉為1000，第2次翻轉為1001
     
255.                                         epwmSetValue(PWM_F,19300,20000);//設置翻轉值，第1次翻轉為1000，第2次翻轉為1001
     
256.                                         epwmSetValue(PWM_B,19300,20000);//設置翻轉值，第1次翻轉為1000，第2次翻轉為1001
     
257.                                         TR0=1;
     
258.                                         EA = 1;
     
259.                                 }
     
260.                         }
     
261. 
     
262.                 }
     
263. }
     
264. 
     
265. void timer0_int (void) interrupt TIMER0_VECTOR
     
266. {
     
267.         B_8ms = 1;
     
268.         P52=1;
     
269. 
     
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]的數據沒有收到 即失聯
     
288.         {
     
289.                 if(++ZT >= 128)
     
290.                 {
     
291.                         ZT = 120;        //狀態標識大于128即1秒沒有收到數據，失控保護
     
292.                        
     
293.                         RxBuf[1] = 128;
     
294.                         RxBuf_temp[1] = 128;
     
295.                         RxBuf[2] = 128; //觸發失控保護 ，緩慢下降，俯仰橫滾方向舵歸中
     
296.                         RxBuf_temp[2]=128;
     
297.                         RxBuf[3] = 128;
     
298.                         RxBuf_temp[3]=128;
     
299.                         if(RxBuf[4] != 0)       
     
300.                         {
     
301.                                 RxBuf[4]--;        //油門在原值逐漸減小
     
302.                                 RxBuf_temp[4]=RxBuf[4];
     
303.                         }
     
304.                 }
     
305.         }
     
306.         else        ZT = 0;
     
307.         lastR0 = RxBuf[0];
     
308. 
     
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);
     
313. 
     
314.         Angle_gx = ((float)(((int *)&tp)[4] - g_x)) / 65.5;        //陀螺儀處理        結果單位是 +-度
     
315.         Angle_gy = ((float)(((int *)&tp)[5] - g_y)) / 65.5;        //陀螺儀量程 +-500度/S, 1度/秒 對應讀數 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;                //儲存上一次角速度數據
     
319.         Last_Angle_gy = Angle_gy;
     
320. 
     
321. 
     
322.        
     
323. //*********************************** 四元數解算 ***********************************
     
324.         IMUupdate(Angle_gx*0.0174533f, Angle_gy*0.0174533f, IMU_gz*0.0174533f, Angle_ax,Angle_ay,Angle_az);//姿態解算，精度0.1度
     
325. //將四軸飛行器的所有數據發到遙控上       
     
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，因為不能從IMU中獲取
     
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 目的是將角度轉弧度        四元數計算出 俯仰(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為遙控給的角度作為基準，RxBuf[8]為微調量，a_y為偏差角度，AngleY為采樣的角度
     
350.         //一般的，在不考慮微調、偏移量，誤差=基準-測量值
     
351.         Ay  = delta_rc_y -(char)RxBuf[8]*2+a_y-AngleY;
     
352.         if(YM > 20)                ERRORY_Out += Ay;                                //外環積分(油門小于某個值時不積分)
     
353.         else                        ERRORY_Out = 0;                                        //油門小于定值時清除積分值
     
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;        //外環PID，“+5”為了四舍五入?
     
360.         Last_Ay=Ay;
     
361.        
     
362.         gy=PID_Output - Angle_gx;
     
363.         if(YM > 20)ERRORY_In +=gy;//內環積分(油門小于某個值時不積分)
     
364.         else                        ERRORY_In = 0;                                                        //油門小于定值時清除積分值
     
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;//加載到速度參數
     
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為遙控給的角度作為基準，RxBuf[7]為微調量，a_x為偏差角度，AngleX為采樣的角度
     
380.         //一般的，在不考慮微調、偏移量，誤差=基準-測量值
     
381.         Ax = delta_rc_x+(char)RxBuf[7]*2+a_x-AngleX;
     
382.         if(YM > 20)        ERRORX_Out += Ax;        //外環積分(油門小于某個值時不積分)
     
383.         else                ERRORX_Out = 0;                //油門小于定值時清除積分值
     
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;        //外環PID
     
390.         Last_Ax=Ax;
     
391.         gx=PID_Output - Angle_gy;
     
392.         if(YM > 20)        ERRORX_In += gx;        //內環積分(油門小于某個值時不積分)
     
393.         else                ERRORX_In  = 0; //油門小于定值時清除積分值
     
394.         if(ERRORX_In >  ERR_MAX)        ERRORX_In =  ERR_MAX;
     
395.         else if(ERRORX_In < -ERR_MAX)        ERRORX_In = -ERR_MAX;        //積分限幅
     
396. 
     
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. //只做速度環即可
     
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;        //本來是乘與65.5的系數，乘與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;
     
422. 
     
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. //**************將速度參數加載至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;        //速度參數控制，防止超過PWM參數范圍0-1000
     
438.                 else if(PWM2 < 0)                PWM2 = 0;
     
439. 
     
440.                 PWM3 = 19300 - (int)YM*4 - speed3;
     
441.                          if(PWM3 > 19300)        PWM3 = 19300;
     
442.                 else if(PWM3 < 0)                PWM3 = 0;
     
443. 
     
444.                 PWM4 = 19300 - (int)YM*4 - speed4;
     
445.                          if(PWM4 > 19300)        PWM4 = 19300;
     
446.                 else if(PWM4 < 0)                PWM4 = 0;
     
447. 
     
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);//設置翻轉值，第1次翻轉為PWM5，第2次翻轉為1001
     
453.         epwmSetValue(PWM_R,PWM2,20000);//設置翻轉值，第1次翻轉為PWM2，第2次翻轉為1001
     
454.         epwmSetValue(PWM_F,PWM4,20000);//設置翻轉值，第1次翻轉為PWM4，第2次翻轉為1001
     
455.         epwmSetValue(PWM_B,PWM3,20000);//設置翻轉值，第1次翻轉為PWM3，第2次翻轉為1001
     
456.         P52=0;
     
457.         }
     
458. void timer3_int (void) interrupt 19
     
459. {
     
460.                  count++;
     
461.                         if(count==1)
     
462.                         {
     
463.                                 nRF24L01_TxPacket(TxBuf);//發射數據
     
464.                         }
     
465.                         else if(count==3)
     
466.                         {
     
467.                                 SetRX_Mode();
     
468.                         }
     
469.                         else if(count==4)
     
470.                         {                               
     
471.                                 nRF24L01_RxPacket(RxBuf_temp);//接收數據
     
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];//左右微調
     
475.                                         RxBuf[7]=RxBuf_temp[7];//前后微調
     
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));
     
525. 
     
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));
     
548. 
     
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. ……………………
     
559. 
     
560. …………限于本文篇幅 余下代碼請從51黑下載附件…………
     

復制代碼

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

2017-9-4 10:13 上傳

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

---

作者: 41886036    時間: 2017-9-15 15:30
謝謝謝謝樓主

---

作者: 41886036    時間: 2017-9-15 15:48
黑幣不夠下載了

---

作者: 41886036    時間: 2017-9-15 15:48
黑幣不夠下載了

---

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

Powered by Discuz! X3.1

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

亚洲春色中文字幕久久久-三上亚