首页 › 四轴飞行器 › 四軸使用arduino uno + esp8266 + gy80 問題

lkk47 发表于 2015-9-17 22:13:18

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

本帖最后由 lkk47 于 2015-9-17 22:23 编辑

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

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

#include <EasyScheduler.h>


#include <Servo.h>
#include <Wire.h>
#include <I2Cdev.h>
#include <helper_3dmath.h>
#include <MPU6050_6Axis_MotionApps20.h>
#include <PID_v1.h>
#include <PinChangeInt.h>
#include <PinChangeIntConfig.h>


#define ESC_A 9
#define ESC_B 6
#define ESC_C 5
#define ESC_D 3

#define RC_1 13
#define RC_2 12
#define RC_3 11
#define RC_4 10
#define RC_5 8
#define RC_PWR A0

#define ESC_MIN 22
#define ESC_MAX 115
#define ESC_TAKEOFF_OFFSET 20
#define ESC_ARM_DELAY 5000

#define RC_HIGH_CH1 1000
#define RC_LOW_CH1 2000
#define RC_HIGH_CH2 1000
#define RC_LOW_CH2 2000
#define RC_HIGH_CH3 1000
#define RC_LOW_CH3 2000
#define RC_HIGH_CH4 1000
#define RC_LOW_CH4 2000
#define RC_HIGH_CH5 1000
#define RC_LOW_CH5 2000
#define RC_ROUNDING_BASE 50

//#define PITCH_P_VAL 0.4
//#define PITCH_I_VAL 0
//#define PITCH_D_VAL 0
//#define ROLL_P_VAL 0.4
//#define ROLL_I_VAL 0
//#define ROLL_D_VAL 0
//#define YAW_P_VAL 0
//#define YAW_I_VAL 0
//#define YAW_D_VAL 0
//
//#define PITCH_MIN -20
//#define PITCH_MAX 20
//#define ROLL_MIN -20
//#define ROLL_MAX 20
//#define YAW_MIN -180
//#define YAW_MAX 180
//#define PID_PITCH_INFLUENCE 20
//#define PID_ROLL_INFLUENCE 20
//#define PID_YAW_INFLUENCE 20

#define PITCH_P_VAL 3
#define PITCH_I_VAL 0
#define PITCH_D_VAL 1
#define ROLL_P_VAL 3
#define ROLL_I_VAL 0
#define ROLL_D_VAL 0.5
#define YAW_P_VAL 0
#define YAW_I_VAL 0
#define YAW_D_VAL 0

#define PITCH_MIN -100
#define PITCH_MAX 100
#define ROLL_MIN -100
#define ROLL_MAX 100
#define YAW_MIN -180
#define YAW_MAX 180
#define PID_PITCH_INFLUENCE 100
#define PID_ROLL_INFLUENCE 100
#define PID_YAW_INFLUENCE 100



MPU6050 mpu;
uint8_t mpuIntStatus;               
uint8_t devStatus;                  
uint16_t packetSize;                  
uint16_t fifoCount;                  
uint8_t fifoBuffer;   

Quaternion q;                        // quaternion for mpu output
VectorFloat gravity;                   // gravity vector for ypr
float ypr = {0.0f,0.0f,0.0f};       // yaw pitch roll values
float yprLast = {0.0f, 0.0f, 0.0f};
volatile bool mpuInterrupt = false;   
boolean interruptLock = false;
float ch1, ch2, ch3, ch4, ch5;
unsigned long rcLastChange1 = micros();
unsigned long rcLastChange2 = micros();
unsigned long rcLastChange3 = micros();
unsigned long rcLastChange4 = micros();
unsigned long rcLastChange5 = micros();
int velocity;                        
float bal_ac , bal_bd;               
float bal_axes;                     
int va, vb, vc, vd;                  
int v_ac, v_bd;                     
Servo a,b,c,d;
PID pitchReg(&ypr, &bal_bd, &ch2, PITCH_P_VAL, PITCH_I_VAL, PITCH_D_VAL, REVERSE);
PID rollReg(&ypr, &bal_ac, &ch1, ROLL_P_VAL, ROLL_I_VAL, ROLL_D_VAL, REVERSE);
PID yawReg(&ypr, &bal_axes, &ch4, YAW_P_VAL, YAW_I_VAL, YAW_D_VAL, DIRECT);
float ch1Last, ch2Last, ch4Last, velocityLast;
int currentValue = 150;//大於150可以使無法馬達啟動
const int maxValue = 255; //pwm最大值為255
#define DEBUG true
                           
byte index = 0;
const int q1 = 3;    //pwm控制電調1
const int q2 = 10;//pwm控制電調2
const int q3 = 11;//pwm控制電調3
const int q4 = 9;   //pwm控制電調4

const int mpuInt = 2;
Schedular Task1;
Schedular Task2;
long previousMillis = 0;
int on = 0;
int ac_tag = 1;
int init_bd = 0;

void setup()
{
Serial.begin(115200);
pinMode(13,OUTPUT);
digitalWrite(13,LOW);
pidSteup();
pinMode(q1, OUTPUT);
pinMode(q2, OUTPUT);
pinMode(q3, OUTPUT);
pinMode(q4, OUTPUT);
pinMode(mpuInt, OUTPUT);
digitalWrite(mpuInt,HIGH);
sendData("AT+RST\r\n",2000,DEBUG); // reset module
sendData("AT+CWMODE=2\r\n",1000,DEBUG); // configure as access point
sendData("AT+CIFSR\r\n",1000,true); // get ip address
sendData("AT+CIPMUX=1\r\n",1000,DEBUG); // configure for multiple connections
sendData("AT+CIPSERVER=1,80\r\n",1000,DEBUG); // turn on server on port 80

digitalWrite(13,HIGH);

Task1.start();
Task2.start();

   

}

void loop()
{
    digitalWrite(13,LOW);
    Task1.check(pidLoop,0);
    Task2.check(receverDataLoop,0);
   
}

void getDateValue(){
   String data = "";
      while (Serial.available() > 0) {
      char a = Serial.read();
      data += a;
      }
   String data_split = getValue(data, ' ', 1);
   Serial.println(data_split);
}

void receverDataLoop(){
if(Serial.available()){
    if(Serial.find("+IPD,")){
   delay(100);
   String data = "";
      while (Serial.available() > 0) {
      char a = Serial.read();
      data += a;
      }
   String data_split_a = getValue(data, '?', 1);
   String data_split_b = getValue(data_split_a, ' ', 0);
   String getData = getValue(data_split_b, '=', 1);
   String key = getValue(data_split_b, '=', 0);
   if(key == "left"){
       Serial.println(getData);
       currentValue = getData.toInt();
   }else if(key == "right"){
       Serial.println(getData);
   }else if(key == "up"){
       on = 1;
       currentValue = getData.toInt();
       if(currentValue < 150){
         on = 0;
       }else{
         on = 1;
         if(init_bd == 0){
            init_bd = bal_bd;
          }
       }
       analogWrite(q1, getData.toInt());
       analogWrite(q2, getData.toInt());
       analogWrite(q3, getData.toInt());
       analogWrite(q4, getData.toInt());
   }
   String closeCommand = "AT+CIPCLOSE";
   Serial.print(closeCommand);
   
    }
}

}

String sendData(String command, const int timeout, boolean debug)
{
    String response = "";
   
    Serial.print(command); // send the read character to the esp8266
   
    long int time = millis();
   
    while( (time+timeout) > millis())
    {
      while(Serial.available())
      {
      
      // The esp has data so display its output to the serial window
      char c = Serial.read(); // read the next character.
      response+=c;
      }
    }
   
    if(debug)
    {
      Serial.print(response);
    }
   
    return response;
}

String getValue(String data, char separator, int index)
{
int found = 0;
int strIndex[] = {
0, -1};
int maxIndex = data.length()-1;
for(int i=0; i<=maxIndex && found<=index; i++){
if(data.charAt(i)==separator || i==maxIndex){
found++;
strIndex = strIndex+1;
strIndex = (i == maxIndex) ? i+1 : i;
}
}
return found>index ? data.substring(strIndex, strIndex) : "";
}

void pidSteup(){
initMPU();
initRegulators();
}

void pidLoop(){
getYPR();                        
computePID();
calculateVelocities();
}

void computePID(){
acquireLock();
ch1 = floor(ch1/RC_ROUNDING_BASE)*RC_ROUNDING_BASE;
ch2 = floor(ch2/RC_ROUNDING_BASE)*RC_ROUNDING_BASE;
ch4 = floor(ch4/RC_ROUNDING_BASE)*RC_ROUNDING_BASE;
ch2 = map(ch2, RC_LOW_CH2, RC_HIGH_CH2, PITCH_MIN, PITCH_MAX);
ch1 = map(ch1, RC_LOW_CH1, RC_HIGH_CH1, ROLL_MIN, ROLL_MAX);
ch4 = map(ch4, RC_LOW_CH4, RC_HIGH_CH4, YAW_MIN, YAW_MAX);
if((ch2 < PITCH_MIN) || (ch2 > PITCH_MAX)) ch2 = ch2Last;
if((ch1 < ROLL_MIN) || (ch1 > ROLL_MAX)) ch1 = ch1Last;
if((ch4 < YAW_MIN) || (ch4 > YAW_MAX)) ch4 = ch4Last;
ch1Last = ch1;
ch2Last = ch2;
ch4Last = ch4;
ypr = ypr * 180/M_PI;
ypr = ypr * 180/M_PI;
ypr = ypr * 180/M_PI;
if(abs(ypr-yprLast)>30) ypr = yprLast;
if(abs(ypr-yprLast)>30) ypr = yprLast;
if(abs(ypr-yprLast)>30) ypr = yprLast;
yprLast = ypr;
yprLast = ypr;
yprLast = ypr;
pitchReg.Compute();
rollReg.Compute();
yawReg.Compute();
releaseLock();
}

void getYPR(){
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
    if((mpuIntStatus & 0x10) || fifoCount >= 1024){
      mpu.resetFIFO();
    }else if(mpuIntStatus & 0x02){
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
      mpu.getFIFOBytes(fifoBuffer, packetSize);
      fifoCount -= packetSize;
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    }
}

//這部分在計算輸出pwm數值可以控制無刷馬達轉速
void calculateVelocities(){
    float bd_a = currentValue;
    float bal_bd_a = bal_bd;
    if(bal_bd_a >= -139 && bal_bd_a <= 0){
      bd_a = (maxValue - currentValue) / 100 * abs(bal_bd) + currentValue;
      if(on == 1){
          analogWrite(q2, bd_a);
      }
      
    }
    float bd_b = currentValue;
    if(bal_bd_a >= 0 && bal_bd_a <=139){
      bd_b = (maxValue - currentValue) / 100 * abs(bal_bd) + currentValue;
      if(on == 1){
      analogWrite(q1, bd_b);
      }
    }
    float ac_a = currentValue;
    if(abs(bal_ac) > 1 && ac_tag == 1){
      ac_tag =1;
    }else if(abs(bal_ac) < 1 && abs(bal_ac) > 0){
      digitalWrite(13,HIGH);
      ac_tag =0;
    }
    float bal_ac_a;
    if(ac_tag == 1){
       bal_ac_a = 0;
    }else{
       bal_ac_a = bal_ac + 1;
    }
    if(bal_ac_a >= -101 && bal_ac_a <= 0){
      ac_a = (maxValue - currentValue) / 100 * abs(bal_ac_a) + currentValue;
       if(on == 1){
         analogWrite(q3, ac_a);
       }
      
    }
    float ac_b = currentValue;
    if(bal_ac_a >= 0 && bal_ac_a <=101){
      ac_b = (maxValue - currentValue) / 100 * abs(bal_ac_a) + currentValue;
      if(on == 1){
      analogWrite(q4, ac_b);
      }
    }
}

inline void updateMotors(){

a.write(va);
c.write(vc);
b.write(vb);
d.write(vd);


}

inline void arm(){

a.write(ESC_MIN);
b.write(ESC_MIN);
c.write(ESC_MIN);
d.write(ESC_MIN);
//delay(ESC_ARM_DELAY);
//delay(10000);

}

inline void dmpDataReady() {
    mpuInterrupt = true;
}

inline void initRC(){
pinMode(RC_PWR, OUTPUT);
digitalWrite(RC_PWR, HIGH);

// FIVE FUCKING INTERRUPTS !!!
PCintPort::attachInterrupt(RC_1, rcInterrupt1, CHANGE);
PCintPort::attachInterrupt(RC_2, rcInterrupt2, CHANGE);
PCintPort::attachInterrupt(RC_3, rcInterrupt3, CHANGE);
PCintPort::attachInterrupt(RC_4, rcInterrupt4, CHANGE);
PCintPort::attachInterrupt(RC_5, rcInterrupt5, CHANGE);

}

void initMPU(){

Wire.begin();
mpu.initialize();
mpu.setMasterClockSpeed(13);
devStatus = mpu.dmpInitialize();
if(devStatus == 0){

    mpu.setDMPEnabled(true);
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    packetSize = mpu.dmpGetFIFOPacketSize();
   
}
}

inline void initESCs(){
a.attach(ESC_A);
b.attach(ESC_B);
c.attach(ESC_C);
d.attach(ESC_D);
delay(100);
arm();
}

inline void initBalancing(){
bal_axes = 0;
bal_ac = 0;
bal_bd = 0;
}

inline void initRegulators(){

pitchReg.SetMode(AUTOMATIC);
pitchReg.SetOutputLimits(-PID_PITCH_INFLUENCE, PID_PITCH_INFLUENCE);

rollReg.SetMode(AUTOMATIC);
rollReg.SetOutputLimits(-PID_ROLL_INFLUENCE, PID_ROLL_INFLUENCE);

yawReg.SetMode(AUTOMATIC);
yawReg.SetOutputLimits(-PID_YAW_INFLUENCE, PID_YAW_INFLUENCE);

}

inline void rcInterrupt1(){
   if(!interruptLock) ch1 = micros() - rcLastChange1;
   rcLastChange1 = micros();
}

inline void rcInterrupt2(){
if(!interruptLock) ch2 = micros() - rcLastChange2;
rcLastChange2 = micros();
}

inline void rcInterrupt3(){
if(!interruptLock) ch3 = micros() - rcLastChange3;
rcLastChange3 = micros();
}

inline void rcInterrupt4(){
if(!interruptLock) ch4 = micros() - rcLastChange4;
rcLastChange4 = micros();
}

inline void rcInterrupt5(){
if(!interruptLock) ch5 = micros() - rcLastChange5;
rcLastChange5 = micros();
}

inline void acquireLock(){
interruptLock = true;
}

inline void releaseLock(){
interruptLock = false;
}

yinveitian 发表于 2015-9-25 17:34:22

学习一下！:)

Demon01 发表于 2017-5-14 12:45:51

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

查看完整版本: 四軸使用arduino uno + esp8266 + gy80 問題