Construction Réglage/ Problème Quadcopter Arduino

#ifndef QUADARDU
#define QUADARDU

#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>



/*  Arduino Pin configuration
 *  
 */

#define ESC_A 26
#define ESC_B 22
#define ESC_C 28
#define ESC_D 24

#define RC_1 46
#define RC_2 44 
#define RC_3 42
#define RC_4 48
#define RC_5 8
#define RC_PWR A0


/* ESC configuration
 *
 */

#define ESC_MIN 20
#define ESC_MAX 120
#define ESC_TAKEOFF_OFFSET 0
#define ESC_ARM_DELAY 5000

/* RC configuration
 * 
 */

#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

/*  PID configuration
 *  
 */

#define PITCH_P_VAL 4
#define PITCH_I_VAL 0.01
#define PITCH_D_VAL 20

#define ROLL_P_VAL 4
#define ROLL_I_VAL 0.01
#define ROLL_D_VAL 20

#define YAW_P_VAL 0
#define YAW_I_VAL 0
#define YAW_D_VAL 0


/* Flight parameters
 *
 */

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


/*  MPU variables
 *
 */

MPU6050 mpu;                           // mpu interface object


uint8_t mpuIntStatus;                  // mpu statusbyte
uint8_t devStatus;                     // device status    
uint16_t packetSize;                   // estimated packet size  
uint16_t fifoCount;                    // fifo buffer size   
uint8_t fifoBuffer[64];                // fifo buffer 

Quaternion q;                          // quaternion for mpu output
VectorFloat gravity;                   // gravity vector for ypr
float ypr[3] = {0.0f,0.0f,0.0f};       // yaw pitch roll values
float yprLast[3] = {0.0f, 0.0f, 0.0f};


volatile bool mpuInterrupt = false;    //interrupt flag

long temps;
 
boolean interruptLock = false;

float ch1, ch2, ch3, ch4, ch5;         // RC channel inputs


int heightOrder;
int pitchOrder;
int rollOrder;
int yawOrder;

float heightSensor;
float pitchSensor;
float rollSensor;
float yawSensor;

float heightSensorLast;
float pitchSensorLast;
float rollSensorLast;
float yawSensorLast;


int velocity;                          // global velocity

float bal_ac, bal_bd;                 // motor balances can vary between -100 & 100
float bal_axes;                       // throttle balance between axes -100:ac , +100:bd

int va, vb, vc, vd;                    //velocities
int v_ac, v_bd;                        // velocity of axes

Servo a,b,c,d;


PID pitchReg(&pitchSensor, &bal_bd, &ch2, PITCH_P_VAL, PITCH_I_VAL, PITCH_D_VAL, DIRECT);
PID rollReg(&rollSensor, &bal_ac, &ch1, ROLL_P_VAL, ROLL_I_VAL, ROLL_D_VAL, REVERSE);
PID yawReg(&yawSensor, &bal_axes, &ch4, YAW_P_VAL, YAW_I_VAL, YAW_D_VAL, DIRECT);
 
float ch1Last, ch2Last, ch4Last, velocityLast;

void setup(){
  
  //receptor init
  pinMode(47, OUTPUT);
  digitalWrite(47,HIGH);
  pinMode(42, INPUT);
  pinMode(44, INPUT);
  pinMode(46, INPUT);
  pinMode(48, INPUT);
  pinMode(52, INPUT);
  delay(2000);
  
  initMPU();
  initESCs();
  initBalancing();
  initRegulators();
  
  Serial.begin(115200);  
  Serial.println("Initialisation effectué");
  temps = millis();
  
}

/* loop function
 *
 */

void loop(){
  
  //heightOrder = map(pulseIn(42, HIGH, 25000),1000,2000,0,180);
  if (digitalRead(52) == HIGH) {
  if (heightOrder < 60) heightOrder = heightOrder + 1;
  if (heightOrder > 60) heightOrder = 60;
  Serial.println("En marche");
  }
  if (digitalRead(52) == LOW) {
  heightOrder = 20;
  Serial.println("A l'arret");
  }  
  pitchOrder = map(pulseIn(44, HIGH, 25000),1000,2000,-90,90);
  rollOrder = map(pulseIn(46, HIGH, 25000),1000,2000,-90,90);
  yawOrder = map(pulseIn(40, HIGH, 25000),1000,2000,-90,90);
  
  while(!mpuInterrupt && fifoCount < packetSize){
     
    /* Do nothing while MPU is not working
     * This should be a VERY short period
     */
      
  }
  
  getYPR();
//  Serial.print("pitchSensor = "); // Afficher les lectures Arduino
//  Serial.println(pitchSensor);
//  Serial.print("rollSensor = "); // Afficher les lectures Arduino
//  Serial.println(rollSensor);
//  Serial.print("yawSensor = "); // Afficher les lectures Arduino
//  Serial.println(yawSensor);
//  Serial.println(" ");
  computePID();
  calculateVelocities();
  updateMotors();
  Serial.println(millis()-temps);
  temps = millis();
  
}

/*  computePID function
 *
 *  formats data for use with PIDLib
 *  and computes PID output
 */

void computePID(){

  acquireLock();

  ch1 = rollOrder;
  ch2 = pitchOrder;
  ch4 = yawOrder;
  
  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;

  pitchReg.Compute();
  rollReg.Compute();
  yawReg.Compute();
  
  releaseLock();

}

/*  getYPR function
 *
 *  gets data from MPU and
 *  computes pitch, roll, yaw on the MPU's DMP
 */

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);
      yawSensor = (ypr[0] * 180/M_PI);
      pitchSensor = (ypr[2] * 180/M_PI);
      rollSensor = (ypr[1] * 180/M_PI);
      
//      if(abs(pitchSensor-pitchSensorLast)>30) pitchSensor[0] = yprLast[0];
//      if(abs(ypr[1]-yprLast[1])>30) ypr[1] = yprLast[1];
//      if(abs(ypr[2]-yprLast[2])>30) ypr[2] = yprLast[2];
//      yprLast[0] = ypr[0];
//      yprLast[1] = ypr[1];
//      yprLast[2] = ypr[2];
    
    }

}

/*  calculateVelocities function
 *  
 *  calculates the velocities of every motor
 *  using the PID output
 */

void calculateVelocities(){

  acquireLock();

  //ch3 = floor(ch3/RC_ROUNDING_BASE)*RC_ROUNDING_BASE;
  //velocity = map(ch3, RC_LOW_CH3, RC_HIGH_CH3, ESC_MIN, ESC_MAX);
  velocity = heightOrder;
  
  
  releaseLock();

  if((velocity < ESC_MIN) || (velocity > ESC_MAX)) velocity = velocityLast;
  
  velocityLast = velocity;
  
  v_ac = (abs(-100+bal_axes)/100)*velocity;
  v_bd = ((100+bal_axes)/100)*velocity;
  
  va = ((100+bal_ac)/100)*v_ac;
  vb = ((100+bal_bd)/100)*v_bd;
  
  vc = (abs((-100+bal_ac)/100))*v_ac;
  vd = (abs((-100+bal_bd)/100))*v_bd;
  
  //Serial.println(bal_bd);
  
  if(velocity < ESC_TAKEOFF_OFFSET){
  
    va = ESC_MIN;
    vb = ESC_MIN;
    vc = ESC_MIN;
    vd = ESC_MIN;
  
  }
  
}

void updateMotors(){

  a.write(va);
  c.write(vc);
  b.write(vb);
  d.write(vd);
//  Serial.println("Vitesse a");
//  Serial.println(va);
//  Serial.println("Vitesse b");
//  Serial.println(vb);
//  Serial.println("Vitesse c");
//  Serial.println(vc);
//  Serial.println("Vitesse d");
//  Serial.println(vd);
  

}


void dmpDataReady() {
    mpuInterrupt = true;
}


void initMPU(){
  
  Wire.begin();
  mpu.initialize();
  devStatus = mpu.dmpInitialize();
  if(devStatus == 0){
  
    mpu.setDMPEnabled(true);
    attachInterrupt(2, dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }  
}


void initESCs(){
  a.write(ESC_MAX);
  b.write(ESC_MAX);
  c.write(ESC_MAX);
  d.write(ESC_MAX);
  a.attach(ESC_A);
  b.attach(ESC_B);
  c.attach(ESC_C);
  d.attach(ESC_D);
  delay(5000);
  a.write(ESC_MIN);
  b.write(ESC_MIN);
  c.write(ESC_MIN);
  d.write(ESC_MIN);
  delay(5000);
}

void initBalancing(){

  bal_axes = 0;
  bal_ac = 0;
  bal_bd = 0;

}

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);

}


void acquireLock(){
  interruptLock = true; 
}

void releaseLock(){
  interruptLock = false;
}

#endif