四軸使用arduino uno + esp8266 + gy80 問題

lkk47

目前使用android app 透過http get方式將控制指令傳送至esp8266 再傳至arduino uno ，接收指令後透過pwm訊號控制電調啟動無刷馬達， 目前遇到的問題在於四軸飛行平衡我是使用gy80 九軸模組並且計算pid但是數值不穩定，我貼上代碼有興趣可以一起研究看看。

github :  https://github.com/billuser/Quadcopter_arduino

1. #include <EasyScheduler.h>
   
2. 
   
3. 
   
4. #include <Servo.h>
   
5. #include <Wire.h>
   
6. #include <I2Cdev.h>
   
7. #include <helper_3dmath.h>
   
8. #include <MPU6050_6Axis_MotionApps20.h>
   
9. #include <PID_v1.h>
   
10. #include <PinChangeInt.h>
    
11. #include <PinChangeIntConfig.h>
    
12. 
    
13. 
    
14. #define ESC_A 9
    
15. #define ESC_B 6
    
16. #define ESC_C 5
    
17. #define ESC_D 3
    
18. 
    
19. #define RC_1 13
    
20. #define RC_2 12
    
21. #define RC_3 11
    
22. #define RC_4 10
    
23. #define RC_5 8
    
24. #define RC_PWR A0
    
25. 
    
26. #define ESC_MIN 22
    
27. #define ESC_MAX 115
    
28. #define ESC_TAKEOFF_OFFSET 20
    
29. #define ESC_ARM_DELAY 5000
    
30. 
    
31. #define RC_HIGH_CH1 1000
    
32. #define RC_LOW_CH1 2000
    
33. #define RC_HIGH_CH2 1000
    
34. #define RC_LOW_CH2 2000
    
35. #define RC_HIGH_CH3 1000
    
36. #define RC_LOW_CH3 2000
    
37. #define RC_HIGH_CH4 1000
    
38. #define RC_LOW_CH4 2000
    
39. #define RC_HIGH_CH5 1000
    
40. #define RC_LOW_CH5 2000
    
41. #define RC_ROUNDING_BASE 50
    
42. 
    
43. //#define PITCH_P_VAL 0.4
    
44. //#define PITCH_I_VAL 0
    
45. //#define PITCH_D_VAL 0
    
46. //#define ROLL_P_VAL 0.4
    
47. //#define ROLL_I_VAL 0
    
48. //#define ROLL_D_VAL 0
    
49. //#define YAW_P_VAL 0
    
50. //#define YAW_I_VAL 0
    
51. //#define YAW_D_VAL 0
    
52. //
    
53. //#define PITCH_MIN -20
    
54. //#define PITCH_MAX 20
    
55. //#define ROLL_MIN -20
    
56. //#define ROLL_MAX 20
    
57. //#define YAW_MIN -180
    
58. //#define YAW_MAX 180
    
59. //#define PID_PITCH_INFLUENCE 20  
    
60. //#define PID_ROLL_INFLUENCE 20
    
61. //#define PID_YAW_INFLUENCE 20
    
62. 
    
63. #define PITCH_P_VAL 3
    
64. #define PITCH_I_VAL 0
    
65. #define PITCH_D_VAL 1
    
66. #define ROLL_P_VAL 3
    
67. #define ROLL_I_VAL 0
    
68. #define ROLL_D_VAL 0.5
    
69. #define YAW_P_VAL 0
    
70. #define YAW_I_VAL 0
    
71. #define YAW_D_VAL 0
    
72. 
    
73. #define PITCH_MIN -100
    
74. #define PITCH_MAX 100
    
75. #define ROLL_MIN -100
    
76. #define ROLL_MAX 100
    
77. #define YAW_MIN -180
    
78. #define YAW_MAX 180
    
79. #define PID_PITCH_INFLUENCE 100
    
80. #define PID_ROLL_INFLUENCE 100
    
81. #define PID_YAW_INFLUENCE 100
    
82. 
    
83. 
    
84. 
    
85. MPU6050 mpu;
    
86. uint8_t mpuIntStatus;                 
    
87. uint8_t devStatus;                    
    
88. uint16_t packetSize;                    
    
89. uint16_t fifoCount;                    
    
90. uint8_t fifoBuffer[64];     
    
91. 
    
92. Quaternion q;                          // quaternion for mpu output
    
93. VectorFloat gravity;                   // gravity vector for ypr
    
94. float ypr[3] = {0.0f,0.0f,0.0f};       // yaw pitch roll values
    
95. float yprLast[3] = {0.0f, 0.0f, 0.0f};
    
96. volatile bool mpuInterrupt = false;   
    
97. boolean interruptLock = false;
    
98. float ch1, ch2, ch3, ch4, ch5;  
    
99. unsigned long rcLastChange1 = micros();
    
100. unsigned long rcLastChange2 = micros();
     
101. unsigned long rcLastChange3 = micros();
     
102. unsigned long rcLastChange4 = micros();
     
103. unsigned long rcLastChange5 = micros();
     
104. int velocity;                          
     
105. float bal_ac , bal_bd;                 
     
106. float bal_axes;                       
     
107. int va, vb, vc, vd;                  
     
108. int v_ac, v_bd;                     
     
109. Servo a,b,c,d;
     
110. PID pitchReg(&ypr[1], &bal_bd, &ch2, PITCH_P_VAL, PITCH_I_VAL, PITCH_D_VAL, REVERSE);
     
111. PID rollReg(&ypr[2], &bal_ac, &ch1, ROLL_P_VAL, ROLL_I_VAL, ROLL_D_VAL, REVERSE);
     
112. PID yawReg(&ypr[0], &bal_axes, &ch4, YAW_P_VAL, YAW_I_VAL, YAW_D_VAL, DIRECT);
     
113. float ch1Last, ch2Last, ch4Last, velocityLast;
     
114. int currentValue = 150;  //大於150可以使無法馬達啟動
     
115. const int maxValue = 255; //pwm最大值為255
     
116. #define DEBUG true
     
117.                              
     
118. byte index = 0;
     
119. const int q1 = 3;    //pwm控制電調1
     
120. const int q2 = 10;  //pwm控制電調2
     
121. const int q3 = 11;  //pwm控制電調3
     
122. const int q4 = 9;   //pwm控制電調4
     
123. 
     
124. const int mpuInt = 2;
     
125. Schedular Task1;
     
126. Schedular Task2;
     
127. long previousMillis = 0;
     
128. int on = 0;
     
129. int ac_tag = 1;
     
130. int init_bd = 0;
     
131. 
     
132. void setup()
     
133. {
     
134.   Serial.begin(115200);
     
135.   pinMode(13,OUTPUT);
     
136.   digitalWrite(13,LOW);
     
137.   pidSteup();
     
138.   pinMode(q1, OUTPUT);
     
139.   pinMode(q2, OUTPUT);
     
140.   pinMode(q3, OUTPUT);
     
141.   pinMode(q4, OUTPUT);
     
142.   pinMode(mpuInt, OUTPUT);
     
143.   digitalWrite(mpuInt,HIGH);
     
144.   sendData("AT+RST\r\n",2000,DEBUG); // reset module
     
145.   sendData("AT+CWMODE=2\r\n",1000,DEBUG); // configure as access point
     
146.   sendData("AT+CIFSR\r\n",1000,true); // get ip address
     
147.   sendData("AT+CIPMUX=1\r\n",1000,DEBUG); // configure for multiple connections
     
148.   sendData("AT+CIPSERVER=1,80\r\n",1000,DEBUG); // turn on server on port 80
     
149.   
     
150.   digitalWrite(13,HIGH);
     
151.   
     
152.   Task1.start();
     
153.   Task2.start();
     
154.   
     
155.    
     
156.   
     
157. }
     
158. 
     
159. void loop()
     
160. {
     
161.     digitalWrite(13,LOW);
     
162.     Task1.check(pidLoop,0);
     
163.     Task2.check(receverDataLoop,0);
     
164.    
     
165. }
     
166. 
     
167. void getDateValue(){
     
168.      String data = "";
     
169.       while (Serial.available() > 0) {
     
170.         char a = Serial.read();
     
171.         data += a;
     
172.       }
     
173.      String data_split = getValue(data, ' ', 1);
     
174.      Serial.println(data_split);
     
175. }
     
176. 
     
177. void receverDataLoop(){
     
178.   if(Serial.available()){
     
179.     if(Serial.find("+IPD,")){
     
180.      delay(100);
     
181.      String data = "";
     
182.       while (Serial.available() > 0) {
     
183.         char a = Serial.read();
     
184.         data += a;
     
185.       }
     
186.      String data_split_a = getValue(data, '?', 1);
     
187.      String data_split_b = getValue(data_split_a, ' ', 0);
     
188.      String getData = getValue(data_split_b, '=', 1);
     
189.      String key = getValue(data_split_b, '=', 0);
     
190.      if(key == "left"){
     
191.        Serial.println(getData);
     
192.        currentValue = getData.toInt();
     
193.      }else if(key == "right"){
     
194.        Serial.println(getData);
     
195.      }else if(key == "up"){
     
196.        on = 1;
     
197.        currentValue = getData.toInt();
     
198.        if(currentValue < 150){
     
199.          on = 0;
     
200.        }else{
     
201.          on = 1;
     
202.          if(init_bd == 0){
     
203.             init_bd = bal_bd;
     
204.           }
     
205.        }
     
206.        analogWrite(q1, getData.toInt());
     
207.        analogWrite(q2, getData.toInt());
     
208.        analogWrite(q3, getData.toInt());
     
209.        analogWrite(q4, getData.toInt());
     
210.      }
     
211.      String closeCommand = "AT+CIPCLOSE";
     
212.      Serial.print(closeCommand);
     
213.    
     
214.     }
     
215.   }
     
216. 
     
217. }
     
218. 
     
219. String sendData(String command, const int timeout, boolean debug)
     
220. {
     
221.     String response = "";
     
222.    
     
223.     Serial.print(command); // send the read character to the esp8266
     
224.    
     
225.     long int time = millis();
     
226.    
     
227.     while( (time+timeout) > millis())
     
228.     {
     
229.       while(Serial.available())
     
230.       {
     
231.         
     
232.         // The esp has data so display its output to the serial window
     
233.         char c = Serial.read(); // read the next character.
     
234.         response+=c;
     
235.       }  
     
236.     }
     
237.    
     
238.     if(debug)
     
239.     {
     
240.       Serial.print(response);
     
241.     }
     
242.    
     
243.     return response;
     
244. }
     
245. 
     
246. String getValue(String data, char separator, int index)
     
247. {
     
248. int found = 0;
     
249.   int strIndex[] = {
     
250. 0, -1  };
     
251.   int maxIndex = data.length()-1;
     
252.   for(int i=0; i<=maxIndex && found<=index; i++){
     
253.   if(data.charAt(i)==separator || i==maxIndex){
     
254.   found++;
     
255.   strIndex[0] = strIndex[1]+1;
     
256.   strIndex[1] = (i == maxIndex) ? i+1 : i;
     
257.   }
     
258. }
     
259.   return found>index ? data.substring(strIndex[0], strIndex[1]) : "";
     
260. }
     
261. 
     
262. void pidSteup(){
     
263.   initMPU();
     
264.   initRegulators();
     
265. }
     
266. 
     
267. void pidLoop(){
     
268.   getYPR();                          
     
269.   computePID();
     
270.   calculateVelocities();
     
271. }
     
272. 
     
273. void computePID(){
     
274.   acquireLock();
     
275.   ch1 = floor(ch1/RC_ROUNDING_BASE)*RC_ROUNDING_BASE;
     
276.   ch2 = floor(ch2/RC_ROUNDING_BASE)*RC_ROUNDING_BASE;
     
277.   ch4 = floor(ch4/RC_ROUNDING_BASE)*RC_ROUNDING_BASE;
     
278.   ch2 = map(ch2, RC_LOW_CH2, RC_HIGH_CH2, PITCH_MIN, PITCH_MAX);
     
279.   ch1 = map(ch1, RC_LOW_CH1, RC_HIGH_CH1, ROLL_MIN, ROLL_MAX);
     
280.   ch4 = map(ch4, RC_LOW_CH4, RC_HIGH_CH4, YAW_MIN, YAW_MAX);
     
281.   if((ch2 < PITCH_MIN) || (ch2 > PITCH_MAX)) ch2 = ch2Last;
     
282.   if((ch1 < ROLL_MIN) || (ch1 > ROLL_MAX)) ch1 = ch1Last;
     
283.   if((ch4 < YAW_MIN) || (ch4 > YAW_MAX)) ch4 = ch4Last;
     
284.   ch1Last = ch1;
     
285.   ch2Last = ch2;
     
286.   ch4Last = ch4;
     
287.   ypr[0] = ypr[0] * 180/M_PI;
     
288.   ypr[1] = ypr[1] * 180/M_PI;
     
289.   ypr[2] = ypr[2] * 180/M_PI;
     
290.   if(abs(ypr[0]-yprLast[0])>30) ypr[0] = yprLast[0];
     
291.   if(abs(ypr[1]-yprLast[1])>30) ypr[1] = yprLast[1];
     
292.   if(abs(ypr[2]-yprLast[2])>30) ypr[2] = yprLast[2];
     
293.   yprLast[0] = ypr[0];
     
294.   yprLast[1] = ypr[1];
     
295.   yprLast[2] = ypr[2];
     
296.   pitchReg.Compute();
     
297.   rollReg.Compute();
     
298.   yawReg.Compute();
     
299.   releaseLock();
     
300. }
     
301. 
     
302. void getYPR(){
     
303.     mpuInterrupt = false;
     
304.     mpuIntStatus = mpu.getIntStatus();
     
305.     fifoCount = mpu.getFIFOCount();
     
306.     if((mpuIntStatus & 0x10) || fifoCount >= 1024){
     
307.       mpu.resetFIFO();
     
308.     }else if(mpuIntStatus & 0x02){
     
309.       while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
     
310.       mpu.getFIFOBytes(fifoBuffer, packetSize);
     
311.       fifoCount -= packetSize;
     
312.       mpu.dmpGetQuaternion(&q, fifoBuffer);
     
313.       mpu.dmpGetGravity(&gravity, &q);
     
314.       mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
     
315.     }
     
316. }
     
317. 
     
318. //這部分在計算輸出pwm數值可以控制無刷馬達轉速
     
319. void calculateVelocities(){
     
320.     float bd_a = currentValue;
     
321.     float bal_bd_a = bal_bd;
     
322.     if(bal_bd_a >= -139 && bal_bd_a <= 0){
     
323.         bd_a = (maxValue - currentValue) / 100 * abs(bal_bd) + currentValue;
     
324.         if(on == 1){
     
325.           analogWrite(q2, bd_a);
     
326.         }
     
327.         
     
328.     }
     
329.     float bd_b = currentValue;
     
330.     if(bal_bd_a >= 0 && bal_bd_a <=139){
     
331.       bd_b = (maxValue - currentValue) / 100 * abs(bal_bd) + currentValue;
     
332.       if(on == 1){
     
333.         analogWrite(q1, bd_b);
     
334.       }
     
335.     }
     
336.     float ac_a = currentValue;
     
337.     if(abs(bal_ac) > 1 && ac_tag == 1){
     
338.         ac_tag =  1;
     
339.     }else if(abs(bal_ac) < 1 && abs(bal_ac) > 0){
     
340.       digitalWrite(13,HIGH);
     
341.       ac_tag =  0;
     
342.     }
     
343.     float bal_ac_a;
     
344.     if(ac_tag == 1){
     
345.        bal_ac_a = 0;
     
346.     }else{
     
347.        bal_ac_a = bal_ac + 1;
     
348.     }
     
349.     if(bal_ac_a >= -101 && bal_ac_a <= 0){
     
350.       ac_a = (maxValue - currentValue) / 100 * abs(bal_ac_a) + currentValue;
     
351.        if(on == 1){
     
352.          analogWrite(q3, ac_a);
     
353.        }
     
354.       
     
355.     }
     
356.     float ac_b = currentValue;
     
357.     if(bal_ac_a >= 0 && bal_ac_a <=101){
     
358.       ac_b = (maxValue - currentValue) / 100 * abs(bal_ac_a) + currentValue;
     
359.       if(on == 1){
     
360.         analogWrite(q4, ac_b);
     
361.       }
     
362.     }
     
363. }
     
364. 
     
365. inline void updateMotors(){
     
366. 
     
367.   a.write(va);
     
368.   c.write(vc);
     
369.   b.write(vb);
     
370.   d.write(vd);
     
371.   
     
372. 
     
373. }
     
374. 
     
375. inline void arm(){
     
376. 
     
377.   a.write(ESC_MIN);
     
378.   b.write(ESC_MIN);
     
379.   c.write(ESC_MIN);
     
380.   d.write(ESC_MIN);
     
381.   //delay(ESC_ARM_DELAY);
     
382.   //delay(10000);
     
383. 
     
384. }
     
385. 
     
386. inline void dmpDataReady() {
     
387.     mpuInterrupt = true;
     
388. }
     
389. 
     
390. inline void initRC(){
     
391.   pinMode(RC_PWR, OUTPUT);
     
392.   digitalWrite(RC_PWR, HIGH);
     
393.   
     
394.   // FIVE FUCKING INTERRUPTS !!!
     
395.   PCintPort::attachInterrupt(RC_1, rcInterrupt1, CHANGE);
     
396.   PCintPort::attachInterrupt(RC_2, rcInterrupt2, CHANGE);
     
397.   PCintPort::attachInterrupt(RC_3, rcInterrupt3, CHANGE);
     
398.   PCintPort::attachInterrupt(RC_4, rcInterrupt4, CHANGE);
     
399.   PCintPort::attachInterrupt(RC_5, rcInterrupt5, CHANGE);
     
400.   
     
401. }
     
402. 
     
403. void initMPU(){
     
404.   
     
405.   Wire.begin();
     
406.   mpu.initialize();
     
407.   mpu.setMasterClockSpeed(13);
     
408.   devStatus = mpu.dmpInitialize();
     
409.   if(devStatus == 0){
     
410.   
     
411.     mpu.setDMPEnabled(true);
     
412.     attachInterrupt(0, dmpDataReady, RISING);
     
413.     mpuIntStatus = mpu.getIntStatus();
     
414.     packetSize = mpu.dmpGetFIFOPacketSize();
     
415.    
     
416.   }
     
417. }
     
418. 
     
419. inline void initESCs(){
     
420.   a.attach(ESC_A);
     
421.   b.attach(ESC_B);
     
422.   c.attach(ESC_C);
     
423.   d.attach(ESC_D);
     
424.   delay(100);
     
425.   arm();
     
426. }
     
427. 
     
428. inline void initBalancing(){
     
429.   bal_axes = 0;
     
430.   bal_ac = 0;
     
431.   bal_bd = 0;
     
432. }
     
433. 
     
434. inline void initRegulators(){
     
435. 
     
436.   pitchReg.SetMode(AUTOMATIC);
     
437.   pitchReg.SetOutputLimits(-PID_PITCH_INFLUENCE, PID_PITCH_INFLUENCE);
     
438.   
     
439.   rollReg.SetMode(AUTOMATIC);
     
440.   rollReg.SetOutputLimits(-PID_ROLL_INFLUENCE, PID_ROLL_INFLUENCE);
     
441.   
     
442.   yawReg.SetMode(AUTOMATIC);
     
443.   yawReg.SetOutputLimits(-PID_YAW_INFLUENCE, PID_YAW_INFLUENCE);
     
444. 
     
445. }
     
446. 
     
447. inline void rcInterrupt1(){
     
448.    if(!interruptLock) ch1 = micros() - rcLastChange1;
     
449.    rcLastChange1 = micros();
     
450. }
     
451. 
     
452. inline void rcInterrupt2(){
     
453.   if(!interruptLock) ch2 = micros() - rcLastChange2;
     
454.   rcLastChange2 = micros();
     
455. }
     
456. 
     
457. inline void rcInterrupt3(){
     
458.   if(!interruptLock) ch3 = micros() - rcLastChange3;
     
459.   rcLastChange3 = micros();
     
460. }
     
461. 
     
462. inline void rcInterrupt4(){
     
463.   if(!interruptLock) ch4 = micros() - rcLastChange4;
     
464.   rcLastChange4 = micros();
     
465. }
     
466. 
     
467. inline void rcInterrupt5(){
     
468.   if(!interruptLock) ch5 = micros() - rcLastChange5;
     
469.   rcLastChange5 = micros();
     
470. }
     
471. 
     
472. inline void acquireLock(){
     
473.   interruptLock = true;
     
474. }
     
475. 
     
476. inline void releaseLock(){
     
477.   interruptLock = false;
     
478. }

复制代码

yinveitian

学习一下！

Demon01

你好
請問可以給我你的android app嗎
我也正在做四軸飛行器 目前做好硬體部分
當初不想買遙控器 想挑戰看看用esp8266遙控 不過現在沒有頭緒不知道怎麼開始
拜託了 謝謝