贡献我的四轴代码

camion

1. #include <I2Cdev.h>
   
2. #include <MPU6050.h>
   
3. #include <Wire.h>
   
4. 
   
5. //=============mpu6050 declarations===========
   
6. MPU6050 accelgyro;
   
7. static double Gx_offset = -1.46;//soldered as LENGBEI
   
8. static double Gy_offset = -0.07;
   
9. static double Ax_offset = -0.21;
   
10. static double Ay_offset = -1.7;
    
11. 
    
12. 
    
13. int16_t ax,ay,az;
    
14. int16_t gx,gy,gz;//raw data from sensors
    
15. double aax,aay,aaz,ggx,ggy,ggz;
    
16. double Ax,Ay,Az;//单位 g(9.8m/s^2)
    
17. double Gx,Gy,Gz;//单位 °/s
    
18. 
    
19. //=================mesure declarations=============
    
20. double estima_X = 0.2;
    
21. double estima_Y = 0.2;
    
22. double es_GX;
    
23. double es_GY;
    
24. double fusion_angle_Y;
    
25. double fusion_angle_X;
    
26. double now_error_X, last_error_X, error_diff_X, error_int_X;
    
27. double now_error_Y, last_error_Y, error_diff_Y, error_int_Y;
    
28. double Angel_accX,Angel_accY,Angel_accZ;
    
29. 
    
30. //===============control & accuracy declarations========  
    
31. double basic_power = 5;     // z acc balancing, need calibration each time
    
32. double rot_power_X, rot_power_Y;
    
33. double orientX=0;
    
34. double orientY=0;
    
35. long tic_1,tic_2,tic_3, duration;
    
36. int time_span_milli =20;
    
37. float time_span =0.02;
    
38. int moterPins[4];
    
39. int moterPowers[4];
    
40.    
    
41. void set_Motors(){
    
42.   pinMode(11,OUTPUT);
    
43.   pinMode(10,OUTPUT);
    
44.   pinMode(9,OUTPUT);
    
45.   pinMode(6,OUTPUT);
    
46.   analogWrite(10, 0);
    
47.   analogWrite(6, 0);
    
48.   analogWrite(11,0);
    
49.   analogWrite(9, 0);
    
50.   delay(100);
    
51. }
    
52. 
    
53. void setup()
    
54. {
    
55.   Wire.begin();
    
56.   Serial.begin(9600);
    
57.   set_Motors();
    
58.   accelgyro.initialize();
    
59.   delay(100);
    
60.   autoCalibration();
    
61.   delay(100);
    
62. }
    
63. 
    
64. void autoCalibration(){
    
65.   Gx_offset=0.0;
    
66.   Gy_offset=0.0;
    
67.   double sum_1 =0;
    
68.   double sum_2 =0;
    
69.   double sum_3 =0;
    
70.   double sum_4 =0;
    
71.   for (int i=0; i<500; i++){
    
72.    accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
    
73.    ggx=gx/131.00;
    
74.    ggy=gy/131.00;
    
75.    Ax=ax/16384.00;
    
76.    Ay=ay/16384.00;
    
77.    Az=az/16384.00;  
    
78.    Angel_accX =atan(Ay/sqrt(Ax*Ax+Az*Az))*180/3.14 ;
    
79.    Angel_accY =-atan(Ax/sqrt(Az*Az+Ay*Ay))*180/3.14;   
    
80.    sum_1 = sum_1 + ggx;
    
81.    sum_2 = sum_2 + ggy;
    
82.    sum_3 = sum_3 + Angel_accX;
    
83.    sum_4 = sum_4 + Angel_accY;
    
84.    delay(1);
    
85.   }
    
86.   Gx_offset = sum_1/500;
    
87.   Gy_offset = sum_2/500;
    
88.   Ax_offset = sum_3/500;
    
89.   Ay_offset = sum_4/500;
    
90. //   Serial.print("Gx_offset= ");
    
91. //   Serial.print(sum_1/1000);
    
92. //   Serial.print("  Gy_offset= ");
    
93. //   Serial.println(sum_2/1000);   
    
94. }
    
95. 
    
96. 
    
97. 
    
98. void loop()
    
99. {
    
100.    tic_1=millis();
     
101.    int emer_stat = 0;
     
102.    //------------------------command tests-----------------------------------
     
103.    while (Serial.available()>=1){
     
104.         char datatype = Serial.read();
     
105.         if (datatype == 'S'){
     
106.              digitalWrite(13, HIGH);   
     
107.              delay(1);               
     
108.              digitalWrite(13, LOW);   
     
109.              delay(1);
     
110.         }else if(datatype == 'M'){
     
111.             orientX=0;
     
112.             orientY=0;
     
113.         }else if(datatype == 'z'){
     
114.             orientX=0;
     
115.         }else if(datatype == 'y'){
     
116.             orientY=0;
     
117.         }else if(datatype == 'L'){
     
118.            basic_power = basic_power+10;
     
119.         }else if(datatype == 'R'){
     
120.            basic_power = basic_power-10;
     
121.         }else if(datatype == 'P'){
     
122.            basic_power = basic_power+1;
     
123.         }else if(datatype == 'K'){
     
124.            basic_power = basic_power-1;
     
125.         }else if(datatype == 'Q'){
     
126.            if (orientX <=18){
     
127.              orientX=orientX+2;
     
128.            }         
     
129.         }else if(datatype == 'W'){
     
130.            if (orientX >= -18){
     
131.              orientX=orientX-2;               
     
132.              digitalWrite(13, HIGH);   
     
133.            }
     
134.         }else if(datatype == 'D'){
     
135.            if (orientY <= 18){
     
136.              orientY=orientY+2;
     
137.            }         
     
138.         }else if(datatype == 'G'){
     
139.            if (orientY >=-18){
     
140.              orientY=orientY-2;
     
141.            }         
     
142.         }else if (datatype == 'V'){
     
143.              int value = Serial.parseInt();
     
144.              if ((value<=255)&&(value>=0)){
     
145.                basic_power = value;
     
146.              }
     
147.         }else if (datatype == 'X'){
     
148. 
     
149.         }else if (datatype == 'Y'){
     
150.       
     
151.         }else if (datatype == 'j'){
     
152.           orientY = 10;
     
153.         }else if (datatype == 'k'){
     
154.           orientY = 5;
     
155.         }else if (datatype == 'l'){
     
156.           orientY = -5;
     
157.         }else if (datatype == 'm'){
     
158.           orientY = -10;
     
159.         }else if (datatype == 'n'){
     
160.           orientX = -5;
     
161.         }else if (datatype == 'o'){
     
162.           orientX = 5;
     
163.         }else if (datatype == 'p'){
     
164.           orientX = 10;
     
165.         }else if (datatype == 'y'){
     
166.           orientY = 0;
     
167.         }else if (datatype == 'z'){
     
168.           orientX = 0;
     
169.         }   
     
170.    }
     
171.    
     
172. //--------------------------do NOT touch-----------------------------------------
     
173.    
     
174.    
     
175. // obtaining and filtering Acc angles   
     
176.    accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
     
177.    Ax=ax/16384.00;
     
178.    Ay=ay/16384.00;
     
179.    Az=az/16384.00;  
     
180.    Angel_accX =atan(Ay/sqrt(Ax*Ax+Az*Az))*180/3.14 -Ax_offset; //offset
     
181.    Angel_accY =-atan(Ax/sqrt(Az*Az+Ay*Ay))*180/3.14 -Ay_offset; //offset  
     
182.    estima_X = estima_cal(estima_X, Angel_accX, 0.2);
     
183.    estima_Y = estima_cal(estima_Y, Angel_accY, 0.2);   
     
184. //   Serial.print("estima_X=");
     
185. //   Serial.print(estima_X);
     
186. //   Serial.print("estima_Y=");
     
187. //   Serial.println(estima_Y);      
     
188.    
     
189. // obtaining Gyro angles   without filter simply because I'm lasy
     
190.    ggx=gx/131.00;
     
191.    ggy=gy/131.00;
     
192.    ggz=gz/131.00;
     
193.    Gx=Gx+(ggx-Gx_offset)*time_span;
     
194.    Gy=Gy+(ggy-Gy_offset)*time_span;
     
195.    autoCorrection();
     
196. //   Serial.print("Gx=");
     
197. //   Serial.print(Gx);
     
198. //   Serial.print(" Gy=");
     
199. //   Serial.println(Gy);
     
200. 
     
201. 
     
202. //fusion angles by combining 40% last observe + 30% Acc + 30% Gyro
     
203. 
     
204.   fusion_angle_X = update_Estimat(fusion_angle_X, estima_X, Gx );
     
205.   fusion_angle_Y = update_Estimat(fusion_angle_Y, estima_Y, Gy );
     
206. //   Serial.print("fusion_X=");
     
207. //   Serial.print(fusion_angle_X);
     
208. //   Serial.print("fusion_Y=");
     
209. //   Serial.println(fusion_angle_Y);     
     
210.    
     
211. //   if (fusion_angle_Y>=80){
     
212. //             digitalWrite(13, HIGH);   
     
213. //             delay(1);               
     
214. //             digitalWrite(13, LOW);   
     
215. //             delay(1);
     
216. //   }
     
217.    
     
218. //---------------------------------PID's----------------------------------------
     
219.   uodate_error(orientX, orientY ,fusion_angle_X, fusion_angle_Y);
     
220.   
     
221.   double command_Y_raw = PID_control(now_error_Y, error_diff_Y, error_int_Y);
     
222.   double command_X_raw = PID_control(now_error_X, error_diff_X, error_int_X);
     
223.   int command_X = int (command_X_raw);
     
224.   int command_Y = int (command_Y_raw);
     
225.   
     
226.   moterPowers[1] =basic_power + command_X;
     
227.   moterPowers[0] =basic_power - command_X;
     
228.   moterPowers[3] =basic_power + command_Y;
     
229.   moterPowers[2] =basic_power - command_Y;
     
230.    
     
231.   emergency_STOP(emer_stat);
     
232. 
     
233. //---------------------------------fuzzy's----------------------------------------
     
234. //   int command_X = fussy_command (fusion_angle_X);
     
235. //   int command_Y = fussy_command (fusion_angle_Y);
     
236. //  moterPowers[0] =basic_power + command_X + int(orientX);
     
237. //  moterPowers[1] =basic_power - command_X - int(orientX);
     
238. //  moterPowers[2] =basic_power + command_Y + int(orientY);
     
239. //  moterPowers[3] =basic_power - command_Y - int(orientY);
     
240. 
     
241. 
     
242. //----------------------executing state for time_span_milli------------------------
     
243. 
     
244.    for (int i=0; i<=3; i++){
     
245.      if (moterPowers[i]< 0){
     
246.        moterPowers[i] = 0;
     
247.      }else if (moterPowers[i] >255){
     
248.        moterPowers[i] = 255;
     
249.      }
     
250.    }
     
251.    
     
252. //   Serial.print("po 10 = ");
     
253. //   Serial.print(moterPowers[0]);
     
254. //   Serial.print("   po 6 = ");
     
255. //   Serial.print(moterPowers[1]);   
     
256. //   Serial.print("   po 11 = ");
     
257. //   Serial.print(moterPowers[2]);
     
258. //   Serial.print("   po 9 = ");
     
259. //   Serial.println(moterPowers[3]);  
     
260.    
     
261.    analogWrite(10, moterPowers[0]);
     
262.    analogWrite(6, moterPowers[1]);
     
263.    analogWrite(11, moterPowers[2]);
     
264.    analogWrite(9, moterPowers[3]);
     
265. 
     
266.    tic_2=millis();
     
267.    duration = tic_2 - tic_1;
     
268.    if(duration > time_span_milli){
     
269.      duration=0;
     
270.    }
     
271.    //Serial.println(time_span_milli - duration);
     
272.    delay (time_span_milli - duration);
     
273.    
     
274. }
     
275. 
     
276. 
     
277. //--------------------------accesory functions----------------------------------------
     
278. double estima_cal (double estima, double metron, double KG){
     
279.   double result1 = estima + KG*(metron - estima);
     
280.   return result1;
     
281. }
     
282. double PID_control(double error, double error_diff, double error_int ){
     
283.   double P=0.7;
     
284.   double D=0.2;
     
285.   double I=0;
     
286.   double command = P*error + I*error_int + D*error_diff;
     
287.   return command;
     
288. }
     
289. void uodate_error(double consigne_X, double consigne_Y, double mesure_X, double mesure_Y){
     
290.   now_error_X = consigne_X - mesure_X;
     
291.   now_error_Y = consigne_Y - mesure_Y;
     
292.   error_diff_X = (now_error_X - last_error_X)/time_span;
     
293.   error_diff_Y = (now_error_Y - last_error_Y)/time_span;
     
294.   error_int_X = error_int_X + now_error_X*time_span;
     
295.   error_int_Y = error_int_Y + now_error_Y*time_span;
     
296.   last_error_X =now_error_X;
     
297.   last_error_Y =now_error_Y;
     
298. }
     
299. double update_Estimat(double estimate, double metron_1, double metron_2){
     
300.   double temp = 0.4* estimate + 0.3*metron_1 + 0.3*metron_2;
     
301.   return temp;
     
302. }
     
303. void emergency_STOP(int choice){
     
304. if (choice == 0){
     
305.     //do nothing
     
306. }else if(choice == 1){
     
307.    //brutal descending
     
308.     basic_power =5;
     
309. //    moterPowers[1] =5;
     
310. //    moterPowers[2] =5;
     
311. //    moterPowers[3] =5;
     
312. }else if(choice == 2){
     
313.     //douce descending
     
314.     moterPowers[0] = moterPowers[0]-10;
     
315.     moterPowers[1] = moterPowers[1]-10;
     
316.     moterPowers[2] = moterPowers[2]-10;
     
317.     moterPowers[3] = moterPowers[3]-10;
     
318. }
     
319. }
     
320. 
     
321. double PorportionDet(double limA, double limB, double outA, double outB, double x){
     
322.   double a = (x - limB) / (limA-limB);
     
323.   double result = a*outA + (1-a)*outB;
     
324.   return result;
     
325. }
     
326. 
     
327. int fussy_command(double input){
     
328.    double PWM_command;
     
329.    if (input>=45 ){
     
330.      PWM_command = 30;
     
331.    }else if(input<45 & input>=25 ){
     
332.      PWM_command = PorportionDet(45, 25, 30, 10, input );
     
333.    }else if(input<25 & input>=5){
     
334.      PWM_command = PorportionDet(25, 5, 10, 3, input );     
     
335.    }else if(input<5 & input>=-5){
     
336.      PWM_command = PorportionDet(5, -5, 3, -3, input );         
     
337.    }else if(input<-5 &input>=-25){
     
338.      PWM_command = PorportionDet(-5, -25, -3, -10, input );              
     
339.    }else if(input<-25 & input>=-45){
     
340.      PWM_command = PorportionDet(-25, -45, -10, -30, input );         
     
341.    }else if(input<-45){
     
342.      PWM_command =-30;
     
343.    }
     
344.    int regu_pwm =int(PWM_command);
     
345.    return regu_pwm;
     
346. }
     
347. void autoCorrection (){
     
348.   if ( Angel_accX > -1.3 & Angel_accX < 1.3 ){
     
349.     Gx=Angel_accX /2;
     
350.   }
     
351.   if ( Angel_accY > -1.3 & Angel_accY < 1.3 ){
     
352.     Gy=Angel_accY /2;
     
353.   }
     
354. }
     

复制代码

除了PID不太稳定，其他还好

camion

FIGHT

期待。谢谢分享。设备马上到齐了，调试下看看

学慧放弃

谢谢楼主无私的共享，飞得平稳度怎样？？？

camion

不好，容易晃，还需要细调PID参数

wing

{:soso_e179:}{:soso_e179:}{:soso_e179:}{:soso_e179:}{:soso_e179:}{:soso_e179:}{:soso_e179:}
我怎么看不见下面那两张图

tanbocandy

这个要必须顶，有没有视频呢

18192908029

感谢楼主！！能否附上说明呢，新手不会接线。。

努力微笑

能飞不？，如果行，详细教程可以有

Super169

感謝樓主無私分享.
可否提供一點硬件的資料, 以及接線方法, 好讓大家學習學習.

peterllloo

感谢楼主分享的代码啊，我用LED灯测试成功了。不过连接电调似乎还要加上空占比发生程序，还有这个代码里面还没有滤波吧，出来的曲线跳动较大。

axjl_yd

感谢分享，关注中

东尼菌

赞一个，第一个帖子给你了