极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 6076|回复: 2

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

[复制链接]
发表于 2015-9-17 22:13:18 | 显示全部楼层 |阅读模式
本帖最后由 lkk47 于 2015-9-17 22:23 编辑

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

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

  1. #include <EasyScheduler.h>


  2. #include <Servo.h>
  3. #include <Wire.h>
  4. #include <I2Cdev.h>
  5. #include <helper_3dmath.h>
  6. #include <MPU6050_6Axis_MotionApps20.h>
  7. #include <PID_v1.h>
  8. #include <PinChangeInt.h>
  9. #include <PinChangeIntConfig.h>


  10. #define ESC_A 9
  11. #define ESC_B 6
  12. #define ESC_C 5
  13. #define ESC_D 3

  14. #define RC_1 13
  15. #define RC_2 12
  16. #define RC_3 11
  17. #define RC_4 10
  18. #define RC_5 8
  19. #define RC_PWR A0

  20. #define ESC_MIN 22
  21. #define ESC_MAX 115
  22. #define ESC_TAKEOFF_OFFSET 20
  23. #define ESC_ARM_DELAY 5000

  24. #define RC_HIGH_CH1 1000
  25. #define RC_LOW_CH1 2000
  26. #define RC_HIGH_CH2 1000
  27. #define RC_LOW_CH2 2000
  28. #define RC_HIGH_CH3 1000
  29. #define RC_LOW_CH3 2000
  30. #define RC_HIGH_CH4 1000
  31. #define RC_LOW_CH4 2000
  32. #define RC_HIGH_CH5 1000
  33. #define RC_LOW_CH5 2000
  34. #define RC_ROUNDING_BASE 50

  35. //#define PITCH_P_VAL 0.4
  36. //#define PITCH_I_VAL 0
  37. //#define PITCH_D_VAL 0
  38. //#define ROLL_P_VAL 0.4
  39. //#define ROLL_I_VAL 0
  40. //#define ROLL_D_VAL 0
  41. //#define YAW_P_VAL 0
  42. //#define YAW_I_VAL 0
  43. //#define YAW_D_VAL 0
  44. //
  45. //#define PITCH_MIN -20
  46. //#define PITCH_MAX 20
  47. //#define ROLL_MIN -20
  48. //#define ROLL_MAX 20
  49. //#define YAW_MIN -180
  50. //#define YAW_MAX 180
  51. //#define PID_PITCH_INFLUENCE 20  
  52. //#define PID_ROLL_INFLUENCE 20
  53. //#define PID_YAW_INFLUENCE 20

  54. #define PITCH_P_VAL 3
  55. #define PITCH_I_VAL 0
  56. #define PITCH_D_VAL 1
  57. #define ROLL_P_VAL 3
  58. #define ROLL_I_VAL 0
  59. #define ROLL_D_VAL 0.5
  60. #define YAW_P_VAL 0
  61. #define YAW_I_VAL 0
  62. #define YAW_D_VAL 0

  63. #define PITCH_MIN -100
  64. #define PITCH_MAX 100
  65. #define ROLL_MIN -100
  66. #define ROLL_MAX 100
  67. #define YAW_MIN -180
  68. #define YAW_MAX 180
  69. #define PID_PITCH_INFLUENCE 100
  70. #define PID_ROLL_INFLUENCE 100
  71. #define PID_YAW_INFLUENCE 100



  72. MPU6050 mpu;
  73. uint8_t mpuIntStatus;                 
  74. uint8_t devStatus;                    
  75. uint16_t packetSize;                    
  76. uint16_t fifoCount;                    
  77. uint8_t fifoBuffer[64];     

  78. Quaternion q;                          // quaternion for mpu output
  79. VectorFloat gravity;                   // gravity vector for ypr
  80. float ypr[3] = {0.0f,0.0f,0.0f};       // yaw pitch roll values
  81. float yprLast[3] = {0.0f, 0.0f, 0.0f};
  82. volatile bool mpuInterrupt = false;   
  83. boolean interruptLock = false;
  84. float ch1, ch2, ch3, ch4, ch5;  
  85. unsigned long rcLastChange1 = micros();
  86. unsigned long rcLastChange2 = micros();
  87. unsigned long rcLastChange3 = micros();
  88. unsigned long rcLastChange4 = micros();
  89. unsigned long rcLastChange5 = micros();
  90. int velocity;                          
  91. float bal_ac , bal_bd;                 
  92. float bal_axes;                       
  93. int va, vb, vc, vd;                  
  94. int v_ac, v_bd;                     
  95. Servo a,b,c,d;
  96. PID pitchReg(&ypr[1], &bal_bd, &ch2, PITCH_P_VAL, PITCH_I_VAL, PITCH_D_VAL, REVERSE);
  97. PID rollReg(&ypr[2], &bal_ac, &ch1, ROLL_P_VAL, ROLL_I_VAL, ROLL_D_VAL, REVERSE);
  98. PID yawReg(&ypr[0], &bal_axes, &ch4, YAW_P_VAL, YAW_I_VAL, YAW_D_VAL, DIRECT);
  99. float ch1Last, ch2Last, ch4Last, velocityLast;
  100. int currentValue = 150;  //大於150可以使無法馬達啟動
  101. const int maxValue = 255; //pwm最大值為255
  102. #define DEBUG true
  103.                              
  104. byte index = 0;
  105. const int q1 = 3;    //pwm控制電調1
  106. const int q2 = 10;  //pwm控制電調2
  107. const int q3 = 11;  //pwm控制電調3
  108. const int q4 = 9;   //pwm控制電調4

  109. const int mpuInt = 2;
  110. Schedular Task1;
  111. Schedular Task2;
  112. long previousMillis = 0;
  113. int on = 0;
  114. int ac_tag = 1;
  115. int init_bd = 0;

  116. void setup()
  117. {
  118.   Serial.begin(115200);
  119.   pinMode(13,OUTPUT);
  120.   digitalWrite(13,LOW);
  121.   pidSteup();
  122.   pinMode(q1, OUTPUT);
  123.   pinMode(q2, OUTPUT);
  124.   pinMode(q3, OUTPUT);
  125.   pinMode(q4, OUTPUT);
  126.   pinMode(mpuInt, OUTPUT);
  127.   digitalWrite(mpuInt,HIGH);
  128.   sendData("AT+RST\r\n",2000,DEBUG); // reset module
  129.   sendData("AT+CWMODE=2\r\n",1000,DEBUG); // configure as access point
  130.   sendData("AT+CIFSR\r\n",1000,true); // get ip address
  131.   sendData("AT+CIPMUX=1\r\n",1000,DEBUG); // configure for multiple connections
  132.   sendData("AT+CIPSERVER=1,80\r\n",1000,DEBUG); // turn on server on port 80
  133.   
  134.   digitalWrite(13,HIGH);
  135.   
  136.   Task1.start();
  137.   Task2.start();
  138.   
  139.    
  140.   
  141. }

  142. void loop()
  143. {
  144.     digitalWrite(13,LOW);
  145.     Task1.check(pidLoop,0);
  146.     Task2.check(receverDataLoop,0);
  147.    
  148. }

  149. void getDateValue(){
  150.      String data = "";
  151.       while (Serial.available() > 0) {
  152.         char a = Serial.read();
  153.         data += a;
  154.       }
  155.      String data_split = getValue(data, ' ', 1);
  156.      Serial.println(data_split);
  157. }

  158. void receverDataLoop(){
  159.   if(Serial.available()){
  160.     if(Serial.find("+IPD,")){
  161.      delay(100);
  162.      String data = "";
  163.       while (Serial.available() > 0) {
  164.         char a = Serial.read();
  165.         data += a;
  166.       }
  167.      String data_split_a = getValue(data, '?', 1);
  168.      String data_split_b = getValue(data_split_a, ' ', 0);
  169.      String getData = getValue(data_split_b, '=', 1);
  170.      String key = getValue(data_split_b, '=', 0);
  171.      if(key == "left"){
  172.        Serial.println(getData);
  173.        currentValue = getData.toInt();
  174.      }else if(key == "right"){
  175.        Serial.println(getData);
  176.      }else if(key == "up"){
  177.        on = 1;
  178.        currentValue = getData.toInt();
  179.        if(currentValue < 150){
  180.          on = 0;
  181.        }else{
  182.          on = 1;
  183.          if(init_bd == 0){
  184.             init_bd = bal_bd;
  185.           }
  186.        }
  187.        analogWrite(q1, getData.toInt());
  188.        analogWrite(q2, getData.toInt());
  189.        analogWrite(q3, getData.toInt());
  190.        analogWrite(q4, getData.toInt());
  191.      }
  192.      String closeCommand = "AT+CIPCLOSE";
  193.      Serial.print(closeCommand);
  194.    
  195.     }
  196.   }

  197. }

  198. String sendData(String command, const int timeout, boolean debug)
  199. {
  200.     String response = "";
  201.    
  202.     Serial.print(command); // send the read character to the esp8266
  203.    
  204.     long int time = millis();
  205.    
  206.     while( (time+timeout) > millis())
  207.     {
  208.       while(Serial.available())
  209.       {
  210.         
  211.         // The esp has data so display its output to the serial window
  212.         char c = Serial.read(); // read the next character.
  213.         response+=c;
  214.       }  
  215.     }
  216.    
  217.     if(debug)
  218.     {
  219.       Serial.print(response);
  220.     }
  221.    
  222.     return response;
  223. }

  224. String getValue(String data, char separator, int index)
  225. {
  226. int found = 0;
  227.   int strIndex[] = {
  228. 0, -1  };
  229.   int maxIndex = data.length()-1;
  230.   for(int i=0; i<=maxIndex && found<=index; i++){
  231.   if(data.charAt(i)==separator || i==maxIndex){
  232.   found++;
  233.   strIndex[0] = strIndex[1]+1;
  234.   strIndex[1] = (i == maxIndex) ? i+1 : i;
  235.   }
  236. }
  237.   return found>index ? data.substring(strIndex[0], strIndex[1]) : "";
  238. }

  239. void pidSteup(){
  240.   initMPU();
  241.   initRegulators();
  242. }

  243. void pidLoop(){
  244.   getYPR();                          
  245.   computePID();
  246.   calculateVelocities();
  247. }

  248. void computePID(){
  249.   acquireLock();
  250.   ch1 = floor(ch1/RC_ROUNDING_BASE)*RC_ROUNDING_BASE;
  251.   ch2 = floor(ch2/RC_ROUNDING_BASE)*RC_ROUNDING_BASE;
  252.   ch4 = floor(ch4/RC_ROUNDING_BASE)*RC_ROUNDING_BASE;
  253.   ch2 = map(ch2, RC_LOW_CH2, RC_HIGH_CH2, PITCH_MIN, PITCH_MAX);
  254.   ch1 = map(ch1, RC_LOW_CH1, RC_HIGH_CH1, ROLL_MIN, ROLL_MAX);
  255.   ch4 = map(ch4, RC_LOW_CH4, RC_HIGH_CH4, YAW_MIN, YAW_MAX);
  256.   if((ch2 < PITCH_MIN) || (ch2 > PITCH_MAX)) ch2 = ch2Last;
  257.   if((ch1 < ROLL_MIN) || (ch1 > ROLL_MAX)) ch1 = ch1Last;
  258.   if((ch4 < YAW_MIN) || (ch4 > YAW_MAX)) ch4 = ch4Last;
  259.   ch1Last = ch1;
  260.   ch2Last = ch2;
  261.   ch4Last = ch4;
  262.   ypr[0] = ypr[0] * 180/M_PI;
  263.   ypr[1] = ypr[1] * 180/M_PI;
  264.   ypr[2] = ypr[2] * 180/M_PI;
  265.   if(abs(ypr[0]-yprLast[0])>30) ypr[0] = yprLast[0];
  266.   if(abs(ypr[1]-yprLast[1])>30) ypr[1] = yprLast[1];
  267.   if(abs(ypr[2]-yprLast[2])>30) ypr[2] = yprLast[2];
  268.   yprLast[0] = ypr[0];
  269.   yprLast[1] = ypr[1];
  270.   yprLast[2] = ypr[2];
  271.   pitchReg.Compute();
  272.   rollReg.Compute();
  273.   yawReg.Compute();
  274.   releaseLock();
  275. }

  276. void getYPR(){
  277.     mpuInterrupt = false;
  278.     mpuIntStatus = mpu.getIntStatus();
  279.     fifoCount = mpu.getFIFOCount();
  280.     if((mpuIntStatus & 0x10) || fifoCount >= 1024){
  281.       mpu.resetFIFO();
  282.     }else if(mpuIntStatus & 0x02){
  283.       while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
  284.       mpu.getFIFOBytes(fifoBuffer, packetSize);
  285.       fifoCount -= packetSize;
  286.       mpu.dmpGetQuaternion(&q, fifoBuffer);
  287.       mpu.dmpGetGravity(&gravity, &q);
  288.       mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  289.     }
  290. }

  291. //這部分在計算輸出pwm數值可以控制無刷馬達轉速
  292. void calculateVelocities(){
  293.     float bd_a = currentValue;
  294.     float bal_bd_a = bal_bd;
  295.     if(bal_bd_a >= -139 && bal_bd_a <= 0){
  296.         bd_a = (maxValue - currentValue) / 100 * abs(bal_bd) + currentValue;
  297.         if(on == 1){
  298.           analogWrite(q2, bd_a);
  299.         }
  300.         
  301.     }
  302.     float bd_b = currentValue;
  303.     if(bal_bd_a >= 0 && bal_bd_a <=139){
  304.       bd_b = (maxValue - currentValue) / 100 * abs(bal_bd) + currentValue;
  305.       if(on == 1){
  306.         analogWrite(q1, bd_b);
  307.       }
  308.     }
  309.     float ac_a = currentValue;
  310.     if(abs(bal_ac) > 1 && ac_tag == 1){
  311.         ac_tag =  1;
  312.     }else if(abs(bal_ac) < 1 && abs(bal_ac) > 0){
  313.       digitalWrite(13,HIGH);
  314.       ac_tag =  0;
  315.     }
  316.     float bal_ac_a;
  317.     if(ac_tag == 1){
  318.        bal_ac_a = 0;
  319.     }else{
  320.        bal_ac_a = bal_ac + 1;
  321.     }
  322.     if(bal_ac_a >= -101 && bal_ac_a <= 0){
  323.       ac_a = (maxValue - currentValue) / 100 * abs(bal_ac_a) + currentValue;
  324.        if(on == 1){
  325.          analogWrite(q3, ac_a);
  326.        }
  327.       
  328.     }
  329.     float ac_b = currentValue;
  330.     if(bal_ac_a >= 0 && bal_ac_a <=101){
  331.       ac_b = (maxValue - currentValue) / 100 * abs(bal_ac_a) + currentValue;
  332.       if(on == 1){
  333.         analogWrite(q4, ac_b);
  334.       }
  335.     }
  336. }

  337. inline void updateMotors(){

  338.   a.write(va);
  339.   c.write(vc);
  340.   b.write(vb);
  341.   d.write(vd);
  342.   

  343. }

  344. inline void arm(){

  345.   a.write(ESC_MIN);
  346.   b.write(ESC_MIN);
  347.   c.write(ESC_MIN);
  348.   d.write(ESC_MIN);
  349.   //delay(ESC_ARM_DELAY);
  350.   //delay(10000);

  351. }

  352. inline void dmpDataReady() {
  353.     mpuInterrupt = true;
  354. }

  355. inline void initRC(){
  356.   pinMode(RC_PWR, OUTPUT);
  357.   digitalWrite(RC_PWR, HIGH);
  358.   
  359.   // FIVE FUCKING INTERRUPTS !!!
  360.   PCintPort::attachInterrupt(RC_1, rcInterrupt1, CHANGE);
  361.   PCintPort::attachInterrupt(RC_2, rcInterrupt2, CHANGE);
  362.   PCintPort::attachInterrupt(RC_3, rcInterrupt3, CHANGE);
  363.   PCintPort::attachInterrupt(RC_4, rcInterrupt4, CHANGE);
  364.   PCintPort::attachInterrupt(RC_5, rcInterrupt5, CHANGE);
  365.   
  366. }

  367. void initMPU(){
  368.   
  369.   Wire.begin();
  370.   mpu.initialize();
  371.   mpu.setMasterClockSpeed(13);
  372.   devStatus = mpu.dmpInitialize();
  373.   if(devStatus == 0){
  374.   
  375.     mpu.setDMPEnabled(true);
  376.     attachInterrupt(0, dmpDataReady, RISING);
  377.     mpuIntStatus = mpu.getIntStatus();
  378.     packetSize = mpu.dmpGetFIFOPacketSize();
  379.    
  380.   }
  381. }

  382. inline void initESCs(){
  383.   a.attach(ESC_A);
  384.   b.attach(ESC_B);
  385.   c.attach(ESC_C);
  386.   d.attach(ESC_D);
  387.   delay(100);
  388.   arm();
  389. }

  390. inline void initBalancing(){
  391.   bal_axes = 0;
  392.   bal_ac = 0;
  393.   bal_bd = 0;
  394. }

  395. inline void initRegulators(){

  396.   pitchReg.SetMode(AUTOMATIC);
  397.   pitchReg.SetOutputLimits(-PID_PITCH_INFLUENCE, PID_PITCH_INFLUENCE);
  398.   
  399.   rollReg.SetMode(AUTOMATIC);
  400.   rollReg.SetOutputLimits(-PID_ROLL_INFLUENCE, PID_ROLL_INFLUENCE);
  401.   
  402.   yawReg.SetMode(AUTOMATIC);
  403.   yawReg.SetOutputLimits(-PID_YAW_INFLUENCE, PID_YAW_INFLUENCE);

  404. }

  405. inline void rcInterrupt1(){
  406.    if(!interruptLock) ch1 = micros() - rcLastChange1;
  407.    rcLastChange1 = micros();
  408. }

  409. inline void rcInterrupt2(){
  410.   if(!interruptLock) ch2 = micros() - rcLastChange2;
  411.   rcLastChange2 = micros();
  412. }

  413. inline void rcInterrupt3(){
  414.   if(!interruptLock) ch3 = micros() - rcLastChange3;
  415.   rcLastChange3 = micros();
  416. }

  417. inline void rcInterrupt4(){
  418.   if(!interruptLock) ch4 = micros() - rcLastChange4;
  419.   rcLastChange4 = micros();
  420. }

  421. inline void rcInterrupt5(){
  422.   if(!interruptLock) ch5 = micros() - rcLastChange5;
  423.   rcLastChange5 = micros();
  424. }

  425. inline void acquireLock(){
  426.   interruptLock = true;
  427. }

  428. inline void releaseLock(){
  429.   interruptLock = false;
  430. }
复制代码

Quadcopter.ino.zip

3.71 KB, 下载次数: 27

上面程式碼

回复

使用道具 举报

发表于 2015-9-25 17:34:22 | 显示全部楼层
学习一下!
回复 支持 反对

使用道具 举报

发表于 2017-5-14 12:45:51 | 显示全部楼层
你好
請問可以給我你的android app嗎
我也正在做四軸飛行器 目前做好硬體部分
當初不想買遙控器 想挑戰看看用esp8266遙控 不過現在沒有頭緒不知道怎麼開始
拜託了 謝謝
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则

Archiver|联系我们|极客工坊 ( 浙ICP备09023225号 )

GMT+8, 2019-7-17 05:27 , Processed in 0.052575 second(s), 28 queries .

Powered by Discuz! X3.4 Licensed

© 2001-2017 Comsenz Inc.

快速回复 返回顶部 返回列表