Ardunio Drone (Control theory, robotics, servo library help needed)

Hello! My name is joe i write the code for the controls team for a Capstone project here at Eastern Washington University. This code is heavlily inspired by Joop Brokkings fantastic video series on an ardunio drone. I made some modifcations to it to fit our needs and also dicided to try and use the servo library instead of manually writing PWM. On our current body and design itteration it flew okay and stablized in the air getting up to a full minute of flight time. Manual control has never worked, in the code ( section stage , state=1) i have a conditional statement that indicated a change in the setpoint of either pitch or roll however a change in setpoint seems to have no effect on anything. No matter how much we change the gains it seems to only make the system less stable. I assume there is a software issue somwhere.

Im curious if there is something obviously wrong to those who have more experience. Does the writemicrosecounds function actually work for this application?

//90% Referenced from Joop Brokking's Code

//I2C library
#include <Wire.h>
#include <Servo.h>

Servo Q1; //"Quad Rotor 1" (front right) using graph notation
Servo Q2;
Servo Q3;
Servo Q4;
  int Q1_output,Q2_output,Q3_output,Q4_output;
//Timer Variables  
unsigned long timer_channel_1, timer_channel_2, timer_channel_3, timer_channel_4, esc_timer, esc_loop_timer;
//Receiver Variables
byte last_channel_1,last_channel_3, last_channel_4,last_channel_2;
int counter_channel_1,counter_channel_3, counter_channel_5,counter_channel_2;
unsigned long timer_1, timer_2,timer_3,timer_4, current_time;
int throttle,up_down,left_right,button,ch3,ch1,ch2;

//IMU
int gyro_x, gyro_y, gyro_z;
long acc_x, acc_y, acc_z;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
long loop_timer;
int temperature;
float angle_pitch, angle_roll;
float a_pitch,a_roll;
int angle_pitch_buffer, angle_roll_buffer;
boolean set_gyro_angles;
float angle_roll_acc, angle_pitch_acc;
float pitch, roll,yaw;
//PID Variables
  float errorA,errorA_p,iA,dA, errorY,errorY_p,dY, errorP,errorP_p,iP,dP, errorR,errorR_p,iR,dR, outputA,outputY,outputP,outputR;
    //Altitude(Not implemented at this time)
      float spA = 0; //SetPoint
      float kpA = 0;//Proportional Gain
      float kiA = 0;//Integral Gain
      float kdA = 0;//Derivative Gain
      
    //Yaw
      float spY = 0;
      float kpY = 0.5;
      float kdY = 70;
    //Pitch
      float spP = 0;
      float kpP = 0.625;//1.1;//0.7;//0;
      float kiP = 0.004;//0;
      float kdP =110;//180;
    //Roll
      float spR = 0;
      float kpR = 0.51;//0.525;
      float kiR = 0.004;//.008;
      float kdR = 90;//75;

//limits
int fullt = 1430; //Sets the throttle limit (for testing set just above hover)
int mint = 1100;
int control_angle= 3; //how much angle should it pitch and roll when controlled

//state
int state;


//////////////////////////////////////////////////////////////////////
//SETUP
//////////////////////////////////////////////////////////////////////
void setup() {
  //Arduino (Atmega) pins default to inputs, so they don't need to be explicitly declared as inputs
  PCICR |= (1 << PCIE0);                                                    //Set PCIE0 to enable PCMSK0 scan.
  PCMSK0 |= (1 << PCINT0);                                                  //Set PCINT0 (digital input 8) to trigger an interrupt on state change.
  PCMSK0 |= (1 << PCINT1);                                                  //Set PCINT1 (digital input 9)to trigger an interrupt on state change.
  PCMSK0 |= (1 << PCINT2);                                                  //Set PCINT2 (digital input 10)to trigger an interrupt on state change.
  PCMSK0 |= (1 << PCINT3);                                                  //Set PCINT3 (digital input 11)to trigger an interrupt on state change.

  Wire.begin();             //Start the I2C as master
  Serial.begin(57600);      //Start the serial connetion @ 57600bps
  delay(250);               //Give the gyro time to start                                  
  Serial.println("Calibrating Gyro");       
  setup_mpu_6050_registers();      
  for (int cal_int = 0; cal_int < 2000 ; cal_int ++){                  //Run this code 2000 times                           
    read_mpu_6050_data();                                             //Read the raw acc and gyro data from the MPU-6050
    gyro_x_cal += gyro_x;                                              //Add the gyro x-axis offset to the gyro_x_cal variable
    gyro_y_cal += gyro_y;                                              
    gyro_z_cal += gyro_z;                                              
    delay(3);                                                          //Delay 3us to simulate the 250Hz program loop
  }
  gyro_x_cal /= 2000;                                                  //Divide the gyro_x_cal variable by 2000 to get the avarage offset
  gyro_y_cal /= 2000;                                                  
  gyro_z_cal /= 2000; 
  angle_pitch = angle_pitch_acc;                                          //Set the gyro pitch angle equal to the accelerometer pitch angle when the quadcopter is started.
  angle_roll = angle_roll_acc;          
  Q1.attach(6,1000,2000);    //attach esc pins
  Q2.attach(7,1000,2000);
  Q3.attach(4,1000,2000);
  Q4.attach(5,1000,2000);
  Q1.write(1000);
  Q2.write(1000);  
  Q3.write(1000);
  Q4.write(1000);
  delay(5000);    
  Serial.print("Done Calibrating");
  
   
                                        
  //chhipa saty
  loop_timer = micros();                                               //Reset the loop timer
}










//////////////////////////////////////////////////////////////////////
//LOOP
//////////////////////////////////////////////////////////////////////
  
void loop(){                                            
  mpu_update(); //Calculates the new values from the mpu
  
  
  throttle = (throttle*.9)+(ch3*.1);    //filters the raw signal from the controller
  left_right= (left_right*.9)+((ch2)*.1);
  up_down = (up_down*.9)+((ch1)*.1);
  
  
  stage(); //Checks stage and calculated PID

  
  //print_();
  while(micros() - loop_timer < 4000);//Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
  loop_timer = micros();          //Reset the loop timer
  
  // Starts the pulses
  Q1.writeMicroseconds(Q1_output);
  Q2.writeMicroseconds(Q2_output);
  Q3.writeMicroseconds(Q3_output);
  Q4.writeMicroseconds(Q4_output);
  
}




//////////////////////////////////////////////////////////////////////
//Stage
//////////////////////////////////////////////////////////////////////

void stage(){
  //Aborts regardless of state
  if (button <1300){ ///controller button on
    state =0;
    iP = 0;  //resets the integral windup and gyro setpoint 
    iR = 0;
    yaw =0; 
  }
  else{
    state =1;
  }
  if (roll > 45 or roll < -45 or pitch >45 or pitch <-45){
    state = 0;

   Serial.print("Too much angle to dangle");
  }


  
  //States
  if (state ==0){ //shut off
    Q1_output =0;
    Q2_output =0;
    Q3_output =0;
    Q4_output =0;
  }



  
  else if (state ==-1){ //Testing pins for the motors 
    if (throttle > 1800) throttle = 1800;
    if (throttle < 1000) throttle = 1000;
    Q1_output =throttle;
    Q2_output =throttle;
    Q3_output =throttle;
    Q4_output =throttle;
    delay(500);
    Q1_output =throttle;
    Q2_output =0;
    Q3_output =0;
    Q4_output =0;
    delay(1000);
    Q1_output =0;
    Q2_output =throttle;
    Q3_output =0;
    Q4_output =0;
    delay(1000);
    Q1_output =0;
    Q2_output =0;
    Q3_output =throttle;
    Q4_output =0;
    delay(1000);
    Q1_output =0;
    Q2_output =0;
    Q3_output =0;
    Q4_output =throttle;
    delay(1000;
  }



  
  else if (state ==1){ //Hover with 
    if (throttle > fullt) throttle = fullt;
    if (throttle < 1000) throttle = 1000;
    call_pid(); //calls the pid values
    Q1_output =throttle+outputY+outputP+outputR;
    Q2_output =throttle-outputY+outputP-outputR;
    Q3_output =throttle+outputY-outputP-outputR;
    Q4_output =throttle-outputY-outputP+outputR;
    
    if (up_down >100) spP = -control_angle;  //gives minor control to the pitch and roll sticks
    else if (up_down <-100) spP = control_angle;
    else spP=0;
    if (left_right >100) spR = -control_angle;
    else if (left_right <-100) spR = control_angle;
    else spR=0;
    
    if (Q1_output < mint) Q1_output = mint;                                         //Keep the motors running.
    if (Q2_output < mint) Q2_output = mint;                                         
    if (Q3_output < mint) Q3_output = mint;                                         
    if (Q4_output < mint) Q4_output = mint;  

    
    
    if(Q1_output > 2000)Q1_output = 2000;                                           //Limit the pulse to 2000us.
    if(Q2_output > 2000)Q2_output= 2000;                                           
    if(Q3_output > 2000)Q3_output= 2000;                                          
    if(Q4_output > 2000)Q4_output= 2000; 
   
  }
  
}


//////////////////////////////////////////////////////////////////////
//PID algorithm
//////////////////////////////////////////////////////////////////////

void call_pid(){

   //Yaw (PD controller so no i term)
  errorY = spY - yaw;
  dY = errorY - errorY_p;
  errorY_p = errorY;
  outputY = (kpY* errorY) + (kdY*dY);

  //Pitch
  errorP = pitch-spP;
  if (errorP <5 and errorP >-5) iP += errorP;
  else iP=0;
  dP = errorP - errorP_p;
  errorP_p = errorP;
  outputP = (kpP* errorP) + (kiP * iP) + (kdP*dP);

  //Roll
  errorR = spR - roll;
  if (errorR <5 and errorR >-5) iR += errorR;
  else iR =0;
  dR = errorR - errorR_p;
  errorR_p = errorR;
  outputR =(kpR* errorR) + (kiR * iR) + (kdR*dR);
}



//////////////////////////////////////////////////////////////////////
//Print Used for debugging
//////////////////////////////////////////////////////////////////////

void print_(){
  Serial.print(throttle);
  Serial.println(" ");
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// MPU
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 
void read_mpu_6050_data(){                                             //Subroutine for reading the raw gyro and accelerometer data
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x3B);                                                    //Send the requested starting register
  Wire.endTransmission();                                              //End the transmission
  Wire.requestFrom(0x68,14);                                           //Request 14 bytes from the MPU-6050
  while(Wire.available() < 14);                                        //Wait until all the bytes are received
  acc_x = Wire.read()<<8|Wire.read();                                  //Add the low and high byte to the acc_x variable
  acc_y = Wire.read()<<8|Wire.read();                                  //Add the low and high byte to the acc_y variable
  acc_z = Wire.read()<<8|Wire.read();                                  //Add the low and high byte to the acc_z variable
  temperature = Wire.read()<<8|Wire.read();                            //Add the low and high byte to the temperature variable
  gyro_x = Wire.read()<<8|Wire.read();                                 //Add the low and high byte to the gyro_x variable
  gyro_y = Wire.read()<<8|Wire.read();                                 //Add the low and high byte to the gyro_y variable
  gyro_z = Wire.read()<<8|Wire.read();                                 //Add the low and high byte to the gyro_z variable

}

void setup_mpu_6050_registers(){
  //Activate the MPU-6050
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x6B);                                                    //Send the requested starting register
  Wire.write(0x00);                                                    //Set the requested starting register
  Wire.endTransmission();                                              //End the transmission
  //Configure the accelerometer (+/-8g)
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x1C);                                                    //Send the requested starting register
  Wire.write(0x10);                                                    //Set the requested starting register
  Wire.endTransmission();                                              //End the transmission
  //Configure the gyro (500dps full scale)
  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050
  Wire.write(0x1B);                                                    //Send the requested starting register
  Wire.write(0x08);                                                    //Set the requested starting register
  Wire.endTransmission();                                              //End the transmission
}


void mpu_update(){
  read_mpu_6050_data();   
  gyro_x -= gyro_x_cal;                                                //Subtract the offset calibration value from the raw gyro_x value
  gyro_y -= gyro_y_cal;                                                //Subtract the offset calibration value from the raw gyro_y value
  gyro_z -= gyro_z_cal;                                                //Subtract the offset calibration value from the raw gyro_z value
  
  //Gyro angle calculations
  //0.0000611 = 1 / (250Hz / 65.5)
  angle_pitch += gyro_x * 0.0000611;                                   //Calculate the traveled pitch angle and add this to the angle_pitch variable
  angle_roll += gyro_y * 0.0000611;                                    //Calculate the traveled roll angle and add this to the angle_roll variable
  yaw += gyro_z *0.0000611;
  //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians 
  angle_pitch += angle_roll * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the roll angle to the pitch angel (This is important for the gyro not the acceleromter)
  angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the pitch angle to the roll angel
 
  //Accelerometer angle calculations
  angle_roll_acc = -(atan2(acc_x, sqrt(acc_y*acc_y + acc_z*acc_z))*180.0)/(3.1415926);
  angle_pitch_acc = (atan2(acc_y, acc_z)*180.0)/(3.1415926);//Calculate the pitch angle (modified by joe  4/26/21)
  
  //Place the MPU-6050 spirit level and note the values in the following two lines for calibration
  angle_pitch_acc -= 0.0;                                              //Accelerometer calibration value for pitch
  angle_roll_acc -= 0.0;                                               //Accelerometer calibration value for roll

  if(set_gyro_angles){                                                 //If the IMU is already started
    angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;     //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
    angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;        //Correct the drift of the gyro roll angle with the accelerometer roll angle
  }
  else{                                                                //At first start
    angle_pitch = angle_pitch_acc;                                     //Set the gyro pitch angle equal to the accelerometer pitch angle 
    angle_roll = angle_roll_acc;                                       //Set the gyro roll angle equal to the accelerometer roll angle 
    set_gyro_angles = true;                                            //Set the IMU started flag
  }
  
  //To dampen the pitch and roll angles a complementary filter is used (EXTRA filtering, its a weighted average)
  pitch = pitch * 0.9 + angle_pitch * 0.1;   //Take 90% of the output pitch value and add 10% of the raw pitch value
  roll = roll * 0.9 + angle_roll * 0.1;      //Take 90% of the output roll value and add 10% of the raw roll value

  
}




/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Transmittor
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 
// Transmitor conversion Interupt pins avalible on the uno 
ISR(PCINT0_vect){
  current_time = micros();
  //Channel 1=========================================
  if(PINB & B00000001){                                                     //Is input 8 high?
    if(last_channel_1 == 0){                                                //Input 8 changed from 0 to 1.
      last_channel_1 = 1;                                                   //Remember current input state.
      timer_1 = current_time;                                               //Set timer_1 to current_time.
    }
  }
  else if(last_channel_1 == 1){                                             //Input 8 is not high and changed from 1 to 0.
    last_channel_1 = 0;                                                     //Remember current input state.
    ch1 = current_time - timer_1-1500;                             //Channel 1 is current_time - timer_1.
  }
  //Channel 2=========================================
  if(PINB & B00000010 ){                                                    //Is input 9 high?
    if(last_channel_2 == 0){                                                //Input 9 changed from 0 to 1.
      last_channel_2 = 1;                                                   //Remember current input state.
      timer_2 = current_time;                                               //Set timer_2 to current_time.
    }
  }
  else if(last_channel_2 == 1){                                             //Input 9 is not high and changed from 1 to 0.
    last_channel_2 = 0;                                                     //Remember current input state.
    ch2 = current_time - timer_2-1500;                             //Channel 2 is current_time - timer_2.
  }
  //Channel 3=========================================
  if(PINB & B00000100 ){                                                    //Is input 10 high?
    if(last_channel_3 == 0){                                                //Input 10 changed from 0 to 1.
      last_channel_3 = 1;                                                   //Remember current input state.
      timer_3 = current_time;                                               //Set timer_3 to current_time.
    }
  }
  else if(last_channel_3 == 1){                                             //Input 10 is not high and changed from 1 to 0.
    last_channel_3 = 0;                                                     //Remember current input state.
    ch3 = current_time - timer_3;                             //Channel 3 is current_time - timer_3.

  }
  //Channel 4=========================================
  if(PINB & B00001000 ){                                                    //Is input 11 high?
    if(last_channel_4 == 0){                                                //Input 11 changed from 0 to 1.
      last_channel_4 = 1;                                                   //Remember current input state.
      timer_4 = current_time;                                               //Set timer_4 to current_time.
    }
  }
  else if(last_channel_4 == 1){                                             //Input 11 is not high and changed from 1 to 0.
    last_channel_4 = 0;                                                     //Remember current input state.
    button = current_time - timer_4;                             //Channel 4 is current_time - timer_4.
  }
}

I think this is your problem. You apply the PID’s before you add the control input to the setpoints:

Thanks that’s spot on!