STM32小四轴自用调试代码分享 无HMC5883程序

四轴测试代码.7z (247.22 KB, 售价: 1 E币)

2020-5-29 01:24 上传

点击文件名下载附件

部分源码：

1. #include "sys.h"
   
2. #include "nrf24l01.h"
   
3. #include "mpu6050.h"
   
4. #include "motor.h"
   
5. #include "led.h"
   
6. #include "nrfreport.h"
   
7. #include "flycontro.h"
   
8. #include "mpudat.h"
   
9. 
   
10. /**
    
11. 微型四轴代码，2016/1/22,chrain
    
12. 无HMC5883程序
    
13. 
    
14. */
    
15. 
    
16. 
    
17. 
    
18. 
    
19. 
    
20. int U8toInt(u8 a,u8 b);
    
21. void FLY_Contro(void);
    
22. int main(void)
    
23. {
    
24.         int PWM;
    
25. //        PWMA =PWMB =PWMC =PWMD =0;
    
26.         SystemInit();
    
27.         delay_init();
    
28.         LED_Init();  //LED初始化
    
29.   
    
30.         if(NRF24L01_Init())
    
31.         LED1 = LED2 = LED3 = LED4 = 1;
    
32.   delay_ms(1000);
    
33.         LED1 = LED2 = LED3 = LED4 = 1;
    
34.         MPU_Init();  //陀螺仪初始化
    
35. 
    
36.         PID_init();
    
37.         delay_ms(1000);
    
38.   Motor_Init();//电机初始化
    
39.   timer_Init();
    
40. //        MPU6050_offset();
    
41. //        LED1 = LED2 = LED3 = LED4 = 0;
    
42. //      
    
43. //        RxBuf[0]=RxBuf[1]=RxBuf[2]=RxBuf[3]=0;
    
44. //        PWMA = PWMB = PWMC = PWMD =0;
    
45. 
    
46. //        PWM_Init();
    
47. //        PID_init();           //PID参数初始化
    
48.         while(1)
    
49.          {
    
50.          if(RxBuf[0]&0xA0)                   LED1 = LED2 = LED3 = LED4 = 1;
    
51.    else  LED_Circleflsah ();
    
52.                  
    
53.                  
    
54.         if(RxBuf[2]>140|RxBuf[2]<115)
    
55.                 Ycontrdat  =  (125 - RxBuf[2])/12.0;
    
56.         else
    
57.                 Ycontrdat =0;
    
58.       
    
59.   if(RxBuf[1]>130|RxBuf[1]<120)
    
60.                 Xcontrdat  =  (RxBuf[1] - 125)/12.0;
    
61.         else
    
62.                 Xcontrdat =0;
    
63.       
    
64.         TargeContrdat.X        =  U8toInt(RxBuf[5],RxBuf[6])/100.0;
    
65.         TargeContrdat.Y        =  U8toInt(RxBuf[7],RxBuf[8])/100.0;
    
66.         TargeContrdat.Z        =  U8toInt(RxBuf[9],RxBuf[10])/100.0;
    
67.                
    
68. 
    
69. if(RxBuf[0] == 0XA1)      
    
70. {      
    
71.   PID_Kp.X =          U8toInt(RxBuf[11],RxBuf[12])/100.0;
    
72.         PID_Kp.Y =          U8toInt(RxBuf[13],RxBuf[14])/100.0;
    
73.         PID_Kp.Z =          U8toInt(RxBuf[15],RxBuf[16])/100.0;
    
74.                  
    
75.   PID_Ki.X =          U8toInt(RxBuf[17],RxBuf[18])/100.0;
    
76.         PID_Ki.Y =          U8toInt(RxBuf[19],RxBuf[20])/100.0;
    
77.         PID_Ki.Z =          U8toInt(RxBuf[21],RxBuf[22])/100.0;
    
78.                
    
79.         PID_Kd.X =          U8toInt(RxBuf[23],RxBuf[24])/100.0;
    
80.         PID_Kd.Y =          U8toInt(RxBuf[25],RxBuf[26])/100.0;
    
81.         PID_Kd.Z =          U8toInt(RxBuf[27],RxBuf[28])/100.0;         
    
82. }                 
    
83.           delay_ms(20);
    
84.                  
    
85. //        NRF_Report_STATUS(Angle.X*100,Angle.Y*100,Angle.Z*100,Accel.X,Accel.X,Accel.Z,Gyro.X,Gyro.Y,Gyro.Z);
    
86.    NRF_Report_STATUS(Angle.Y*100,Ycontrdat*100,Angle.Z*100,
    
87.                                  Angle.Y*100,Ycontrdat*100,Accel.Z,
    
88.                                  Gyro.X,Gyro.Y,Gyro.Z);
    
89. 
    
90.                 PWM = 256 - RxBuf[3];
    
91. if(RxBuf[0]&0xA0)
    
92. {
    
93.          if(PWM > 30)
    
94.                  {
    
95.                         if(PWM>250) PWM =249;
    
96.          
    
97.        Fly_PWM = (PWM-30)*2.2;
    
98.      }else
    
99.                   {
    
100.                                 Fly_PWM =0;
     
101.                                 Angle.Z =0;
     
102.                         }
     
103. }else
     
104.                   {
     
105.                                 Fly_PWM =0;
     
106.                                 Angle.Z =0;
     
107.                         }
     
108. // Prepare_Data(&Acc,&Acc_AVG);
     
109.   //  IMUupdate(&Gyr,&Acc_AVG,&Angle);        
     
110. 
     
111. //  LED2 =1;
     
112. //  delay_ms(100);
     
113.         //        LED1=~LED1;
     
114.                  
     
115.                  
     
116.                  
     
117. //          NRF24l01_Report_STATUS((int)(Angle.rol *100),(int)(Angle.pit *100),(int)(Angle.yaw*100),0,0);
     
118. //               
     
119. //i++;               
     
120. //if(i>10)
     
121. //{      
     
122. //        TxBuf[15] = (Moto_A>>8)&0x00ff;               
     
123. //  TxBuf[16] = Moto_A&0x00ff;
     
124. //  TxBuf[17] = (Moto_B>>8)&0x00ff;
     
125. //  TxBuf[18] = Moto_B&0x00ff;
     
126. //  TxBuf[19] = (Moto_C>>8)&0x00ff;
     
127. //        TxBuf[20] = Moto_C&0x00ff;
     
128. //  TxBuf[21] = (Moto_D>>8)&0x00ff;
     
129. //  TxBuf[22] = Moto_D&0x00ff;
     
130. //        i=0;
     
131. }
     
132.                
     
133. 
     
134. 
     
135. //   FLY_Contro();
     
136. 
     
137. 
     
138. //         if(RxBuf[1]>0)        
     
139. //                 Fly_PWM = RxBuf[1]*5;
     
140. //                else
     
141. //                 Fly_PWM=0;      
     
142. //               
     
143. //               
     
144. //               
     
145. 
     
146. // }
     
147. }
     
148. 
     
149. 
     
150. 
     
151. 
     
152. void FLY_Contro(void)
     
153. {
     
154. //float RolContro,PitContro;
     
155. //float temp;
     
156. //static float Rol_weitiao=0,Pit_weitiao=0;
     
157. 
     
158. //      
     
159. //      
     
160. //         
     
161. ////PIT控制
     
162. //                 if(RxBuf[5]<115)  
     
163. //                  {
     
164. //                         PitContro = (115-RxBuf[5])/5.0;         
     
165. //                         if(PitContro >= 20) PitContro=20.0;                                 
     
166. //      }else if(RxBuf[5]>125)        //         if(TxBuf[3]<115) //TxBuf[3]>125
     
167. //                  {
     
168. //       PitContro = (125-RxBuf[5])/5.0;                       
     
169. //       if(PitContro<=-20) PitContro=-20;                              
     
170. //      }
     
171. ////ROll控制                       
     
172. //   if(RxBuf[4]<115)  
     
173. //                  {
     
174. //                         RolContro = (115-RxBuf[4])/5.0;         
     
175. //                         if(RolContro >= 20) RolContro=20.0;                                 
     
176. //      }else if(RxBuf[4]>125)        //         if(TxBuf[3]<115) //TxBuf[3]>125
     
177. //                  {
     
178. //       RolContro = (125- RxBuf[4])/5.0;                       
     
179. //       if(RolContro<=-20) RolContro=-20;                              
     
180. //      }
     
181. //                       
     
182. 
     
183. //                       
     
184. //         Roll.Targe   =          RolContro + Rol_weitiao;      
     
185. //         Pitch.Targe   =  -PitContro + Pit_weitiao;               
     
186. //               
     
187. 
     
188. //        if(RxBuf[9]==1)
     
189. // {
     
190. //  temp = U8toInt(RxBuf[10],RxBuf[11]);  //ROll微调
     
191. // //       Pit_PID.Kp  = temp/100.0;
     
192. //              Rol_weitiao= temp/4.0;  
     
193. //        temp = U8toInt(RxBuf[12],RxBuf[13]);  //PIt微调
     
194. //                  Pit_weitiao= temp/4.0;  
     
195. ////        Pit_PID.Kd= temp/100.0;         
     
196. // }
     
197. //        RxBuf[9]=0;                                
     
198. 
     
199. }
     
200. 
     
201. 
     
202. 
     
203. 
     
204. 
     
205. 
     
206. 
     
207. 
     
208. 
     
209. 
     
210. 
     
211. 
     
212. int U8toInt(u8 a,u8 b)
     
213. {
     
214. int temp=0;
     
215. //    b = temp&0x00ff;
     
216. //    a = (temp>>8)&0x00ff;                 
     
217.                 temp = (a<<8)|b;
     
218.     if(temp>32768)temp-=65536;      
     
219.     return temp;
     
220. }
     
221. 
     
222. 
     
223. 
     
224. 
     
225. 
     
226. //void PWM_Init(void)
     
227. //{
     
228. // GPIO_InitTypeDef GPIO_InitStructure;
     
229. ……………………
     
230. 
     
231. …………限于本文篇幅 余下代码请下载附件…………
     

复制代码

【必读】版权免责声明

1、本主题所有言论和内容纯属会员个人意见，与本论坛立场无关。2、本站对所发内容真实性、客观性、可用性不做任何保证也不负任何责任，网友之间仅出于学习目的进行交流。3、对提供的数字内容不拥有任何权利，其版权归原著者拥有。请勿将该数字内容进行商业交易、转载等行为，该内容只为学习所提供，使用后发生的一切问题与本站无关。 4、本网站不保证本站提供的下载资源的准确性、安全性和完整性;同时本网站也不承担用户因使用这些下载资源对自己和他人造成任何形式的损失或伤害。 5、本网站所有软件和资料均为网友推荐收集整理而来，仅供学习用途使用，请务必下载后两小时内删除，禁止商用。6、如有侵犯你版权的，请及时联系我们（电子邮箱1370723259@qq.com）指出，本站将立即改正。