标题: 四轴飞行器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.51hei.com/bbs/)

Powered by Discuz! X3.1