PID control of Quadcopter

Hi I am trying to PID control the Arduino-Quadcopter. I am only controlling throttle roll and pitch movement of my quadcopter. Everything works fine but now I am having trouble in PID control of my quad.

PID r0utput(&compAngleX,&roll, &droll, rkp, rki, rkd, DIRECT);

written that as according to:

PID(&Input, &Output, &Setpoint, Kp, Ki, Kd, Direction)

but the problem is, instead of adjusting output with reference of setpoint, PID function is calculating its output with the reference of “setpoint” and “Input” both.
“Input” is sensor’s output and “setpoint” is desired roll I want to achieve.

In my understandings, PID should have only calculate or change its “output” when setpoint value is changed. not when the “input” is changed. Am I right?
here is the complete new code:

//================================================================
// ===               Sensor Int.                                ===
// ================================================================
#include<Wire.h>
const int MPU=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;//stores 
double compAngleX,compAngleY,timer;
  double accXangle ,accYangle ,gyroXrate ,gyroYrate;
  double gyroXAngle, gyroYAngle;
 
  int ap=0.955;

//------------------------------------------------------------------------------------------\\
////////////////// OTHER Init. //////////////////////////
#include <Wire.h> 
#include <PID_v1.h>

int esc1= 5;//front right
int esc2= 6;//back right 
int esc3= 7;//front left 
int esc4= 8;//back left
double front_left,front_right,back_left,back_right;

double dpitch,droll;
double pitch,roll;
volatile double throttle;
float pkp =0.41, pki= 0, pkd= 0;

float rkp =0.41,rki =0,rkd= 0;


/////////////////////////////////////////////////////////////
PID p0utput(&compAngleY,&pitch, &dpitch,pkp, pki, pkd, DIRECT);
PID r0utput(&compAngleX,&roll, &droll, rkp, rki, rkd, DIRECT);
///////////////////////////////////////////////
/////////////////////////////////////////////////

#define PID_ROLL_INFLUENCE 10
////////////////////////PID DATA END//////////////
//--------------------------------------------------------------\\

////////////////////////RC_RX Init.///////////////////////////


#define CH1_int_th 0    // Channel 1 interrupt throttle 
#define CH1_pin_th 2     // Respective channel Hardware interrupt pin number

#define CH2_int_pt 1     // Channel 2 interrupt pitch 
#define CH2_pin_pt 3     // Respective channel Hardware interrupt pin number

#define CH3_int_r1 4     // Channel 3 interrupt roll 
#define CH3_pin_r1 19     // Respective channel Hardware interrupt pin number

#define valid_pulse_limit 2000 // [uS] Valid output high pulse time limit for RC controller
#define max_high_time 1895 // [uS] Maximum expected high time
#define min_high_time 1090 // [uS] Minimum expected high time
 

unsigned long t=0;
int counter;
void init_rc_rx();
//void read_rc_rx();  

volatile unsigned long CH1_t_th=0, CH1_delta_th=0, CH2_t_pt=0, CH2_delta_pt=0, CH3_t_rl=0, CH3_delta_rl=0 ;

// Interrupt ISRs

void CH1_int_ISR()
{
  
  if ((micros()-CH1_t_th) < valid_pulse_limit){
    CH1_delta_th = micros()-CH1_t_th;
  }
  CH1_t_th = micros();
}

void CH2_int_ISR()
{
  
  if ((micros()-CH2_t_pt) < valid_pulse_limit){
    CH2_delta_pt = micros()-CH2_t_pt;
  }
  CH2_t_pt = micros();
}

void CH3_int_ISR()
{
  
  if ((micros()-CH3_t_rl) < valid_pulse_limit){
    CH3_delta_rl = micros()-CH3_t_rl;
  }
  CH3_t_rl = micros();
}



// Initialization\\

void init_rc_rx(){
  

  //MAKING PINS INPUT//
  pinMode(CH1_pin_th, INPUT);
  pinMode(CH2_pin_pt, INPUT);
  pinMode(CH3_pin_r1, INPUT);

  //ENABLING INTERRUPTS//
  attachInterrupt(CH1_int_th, CH1_int_ISR, CHANGE);
  attachInterrupt(CH2_int_pt, CH2_int_ISR, CHANGE);
  attachInterrupt(CH3_int_r1, CH3_int_ISR, CHANGE);

}

//---------------------------------------------------------------------\\




void pid()
{
  r0utput.SetMode(AUTOMATIC);//set pid on may be
 r0utput.SetOutputLimits(-PID_ROLL_INFLUENCE, PID_ROLL_INFLUENCE);
 
  r0utput.Compute();
  
  p0utput.SetMode(AUTOMATIC);
 p0utput.SetOutputLimits(-PID_ROLL_INFLUENCE, PID_ROLL_INFLUENCE);
 
  p0utput.Compute();
}



// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void sensor_temp() {
    Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,14);  // request a total of 14 registers//wire.requestFrom to request bytes from a slave device.
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
 

  
 accXangle = (atan2(AcY, AcZ) * RAD_TO_DEG);
 accYangle = (atan2(AcX, AcZ) * RAD_TO_DEG);

 gyroXrate = GyX / 16.5;
 gyroYrate = GyY / 16.5;
 
 timer = millis();
 //angular position
 gyroXAngle += gyroXrate * (millis()-timer)/1000;
 gyroYAngle += gyroYrate * (millis()-timer)/1000;
 
 
 //---------------------------\\
 //COMPLIMENTRY FILTER STARTED\\
 //---------------------------\\

 compAngleX = ap * (compAngleX + gyroXAngle) + (1-ap) * accXangle;
 compAngleY = ap * (compAngleY + gyroYAngle) + (1-ap) * accYangle;
 
 //---------------------------\\
 //COMPLIMENTRY FILTER ENDED  \\
 //---------------------------\\

}

void setup() {
  
  pinMode(esc1,OUTPUT); 
  pinMode(esc2,OUTPUT);
  pinMode(esc3,OUTPUT);	
  pinMode(esc4,OUTPUT);

  init_rc_rx();

// ================================================================
// ===                     SENSOR   SETUP                       ===
// ================================================================
 Serial.begin(115200);
/////////////////////// SENSOR READING//////////
 Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  ///////////////////////////////////////

}

void loop() {

sensor_temp();
pid();


  ///////////////////////////looping/////////////////
      


throttle=map(CH1_delta_th,988,1968,0,200);
throttle=constrain(throttle,0,200);
Serial.println("");
Serial.println("");
Serial.println("");
Serial.print("throttle Rcv");
Serial.println(CH1_delta_th);
Serial.print("Roll Rcv");
Serial.println(CH3_delta_rl);
Serial.print("Pitch Rcv");
Serial.println(CH2_delta_pt);
Serial.println("");
Serial.println("");
Serial.println("");



droll=map(CH3_delta_rl,1008,1968,-45,45);
      if(CH3_delta_rl>1470 && CH3_delta_rl<1490)
 { droll=0;
}      
droll=constrain(droll,-45,45);


 dpitch=map(CH2_delta_pt,970,1976,-45,45);
          if(CH2_delta_pt>1460&&CH2_delta_pt<1490)
 { dpitch=0;
}

 dpitch=constrain(dpitch,-45,45);




            if(roll<1.86 && roll>-1.86)
 { roll=0;
}     

            if(pitch<1.86&&pitch>-1.86)
 { pitch=0;
}


Serial.print("pitch ");
Serial.println(pitch);
Serial.print("roll ");
Serial.println(roll);
Serial.print("throttle ");
Serial.println(throttle);

front_left=map(throttle+roll+pitch,0,170,0,200); // front left
front_right=map(throttle-roll+pitch,0,170,0,200);//front right
back_right=map(throttle-roll-pitch,0,170,0,200);//back right
back_left=map(throttle+roll-pitch,0,170,0,200);//back left

front_left=constrain(front_left,0,240); // front left
front_right=constrain(front_right,0,240);//front right
back_right=constrain(back_right,0,240);//back right
back_left=constrain(back_left,0,240);//back left
 
Serial.print("front_left ");
Serial.println(front_left);
Serial.print("front_right ");
Serial.println(front_right);
Serial.print("back_right ");
Serial.println(back_right);
Serial.print("back_left ");
Serial.println(back_left);

//======Writing Values====\\
analogWrite (esc3,front_left);//fl esc3
analogWrite (esc4,back_left);//bl esc4
analogWrite (esc1,front_right); //fr esc1
analogWrite (esc2,back_right);//br 
delay(1000);
    /////////////////////////////////////////////
}

PID calculates output referencing the error between input and setpoint. So if either (or both) input or setpoint change the output will change

ahsanraza: In my understandings, PID should have only calculate or change its "output" when setpoint value is changed. not when the "input" is changed. Am I right?

Your understanding could not be more wrong. A PID REQUIRES the update method to be called repeatedly, at a consistent time interval (the more often the better, up to a point). Otherwise, it becomes little more than an on/off switch, and a very poor one at that.

I'd suggest doing some Googling, to get a good understanding of what a PID is, and how it works. Otherwise, you'll never be ble to tune it to achieve good performance.

Regards, Ry L.

Okay I have googled about the PID and cleared my understanding about the PID. but I am unable to balance my quad.

When sensor senses rolling, by some external force on quad ,for example 15 degree positive, and I have not given any command of rolling the quad. the quad still calculate PWM signals for all motors and rolls the quad according to that value the sensor sensed. All I want, when I have not given any roll or pitch command by the transmitter, the quad balance the external force itself. Can anyone tell how I can achieve that?

Sounds like you have a programming error.

  timer = millis();
  //angular position
  gyroXAngle += gyroXrate * (millis() - timer) / 1000;
  gyroYAngle += gyroYrate * (millis() - timer) / 1000;

This bit isn't going to help much. Most of the time it multiplies your gyro rate by 0.

void pid()
{
  r0utput.SetMode(AUTOMATIC);//set pid on may be
  r0utput.SetOutputLimits(-PID_ROLL_INFLUENCE, PID_ROLL_INFLUENCE);

  r0utput.Compute();

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

  p0utput.Compute();
}

There is no need to set the Mode and the OutputLimits every time you calculate the PID.

It looks like you are calculating dpitch and droll AFTER you are running the PID. Since those are the Setpoint values, shouldn't they be calculated BEFORE you use them?

Thankyou for your Valueable suggestion jhon. I have corrected my mistakes as you suggested. but still I can't PID tune my quad. :(

I have corrected my mistakes

Please post the corrected version of the complete program.

// ================================================================
// ===               Sensor Int.                                ===
// ================================================================
#include<Wire.h>
const int MPU=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;//stores 
double compAngleX,compAngleY,timer;
  double accXangle ,accYangle ,gyroXrate ,gyroYrate;
  double gyroXAngle, gyroYAngle;
 
  int ap=0.955;

//------------------------------------------------------------------------------------------\\
////////////////// OTHER Init. //////////////////////////
#include <Wire.h> 
#include <PID_v1.h>

int esc1= 5;//front right
int esc2= 6;//back right 
int esc3= 7;//front left 
int esc4= 8;//back left
double front_left,front_right,back_left,back_right;

double dpitch,droll;
double pitch,roll;
volatile double throttle;
float pkp =.41, pki= 00, pkd=0;

float rkp =.41,rki =0,rkd= 0;
#define PID_ROLL_INFLUENCE 30

/////////////////////////////////////////////////////////////


PID p0utput(&compAngleY,&pitch, &dpitch,pkp, pki, pkd, DIRECT);
PID r0utput(&compAngleX,&roll, &droll, rkp, rki, rkd, DIRECT);
///////////////////////////////////////////////
/////////////////////////////////////////////////


////////////////////////PID DATA END//////////////
//--------------------------------------------------------------\\

////////////////////////RC_RX Init.///////////////////////////


#define CH1_int_th 0    // Channel 1 interrupt throttle 
#define CH1_pin_th 2     // Respective channel Hardware interrupt pin number

#define CH2_int_pt 1     // Channel 2 interrupt pitch 
#define CH2_pin_pt 3     // Respective channel Hardware interrupt pin number

#define CH3_int_r1 4     // Channel 3 interrupt roll 
#define CH3_pin_r1 19     // Respective channel Hardware interrupt pin number

#define CH4_int 5     // Channel 4 interrupt # 
#define CH4_pin 18    // Respective channel Hardware interrupt pin number
//#define CH5_pin 23
//#define CH6_pin 25

#define valid_pulse_limit 2000 // [uS] Valid output high pulse time limit for RC controller
#define max_high_time 1895 // [uS] Maximum expected high time
#define min_high_time 1090 // [uS] Minimum expected high time
 

unsigned long t=0;
int counter;
void init_rc_rx();
//void read_rc_rx();  

volatile unsigned long CH1_t_th=0, CH1_delta_th=0, CH2_t_pt=0, CH2_delta_pt=0, CH3_t_rl=0, CH3_delta_rl=0 ;
//uncomment them for future use

volatile unsigned long CH4_t=0, CH4_delta=0;
//pulseIn channels for accessory 
//unsigned long CH5_delta=0,CH6_delta=0;

// Interrupt ISRs

void CH1_int_ISR()
{
  
  if ((micros()-CH1_t_th) < valid_pulse_limit){
    CH1_delta_th = micros()-CH1_t_th;
  }
  CH1_t_th = micros();
}

void CH2_int_ISR()
{
  
  if ((micros()-CH2_t_pt) < valid_pulse_limit){
    CH2_delta_pt = micros()-CH2_t_pt;
  }
  CH2_t_pt = micros();
}

void CH3_int_ISR()
{
  
  if ((micros()-CH3_t_rl) < valid_pulse_limit){
    CH3_delta_rl = micros()-CH3_t_rl;
  }
  CH3_t_rl = micros();
}

void CH4_int_ISR()
{
  
  if ((micros()-CH4_t) < valid_pulse_limit){
    CH4_delta = micros()-CH4_t;
  }
  CH4_t = micros();
  
}

// Initialization\\

void init_rc_rx(){
  

  //MAKING PINS INPUT//
  pinMode(CH1_pin_th, INPUT);
  pinMode(CH2_pin_pt, INPUT);
  pinMode(CH3_pin_r1, INPUT);
  pinMode(CH4_pin,INPUT);

  //ENABLING INTERRUPTS//
  attachInterrupt(CH1_int_th, CH1_int_ISR, CHANGE);
  attachInterrupt(CH2_int_pt, CH2_int_ISR, CHANGE);
  attachInterrupt(CH3_int_r1, CH3_int_ISR, CHANGE);
  attachInterrupt(CH4_int,CH4_int_ISR,CHANGE);

}

//---------------------------------------------------------------------\\

void setup() {
  
r0utput.SetMode(AUTOMATIC);//set pid on may be
r0utput.SetOutputLimits(-PID_ROLL_INFLUENCE, PID_ROLL_INFLUENCE);
  
p0utput.SetMode(AUTOMATIC);
p0utput.SetOutputLimits(-PID_ROLL_INFLUENCE, PID_ROLL_INFLUENCE);

  
  pinMode(esc1,OUTPUT); 
  pinMode(esc2,OUTPUT);
  pinMode(esc3,OUTPUT);	
  pinMode(esc4,OUTPUT);

  init_rc_rx();

// ================================================================
// ===                     SENSOR   SETUP                       ===
// ================================================================
 Serial.begin(115200);
/////////////////////// SENSOR READING//////////
 Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  ///////////////////////////////////////
  

}

// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void sensor_temp() {
    Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,14);  // request a total of 14 registers//wire.requestFrom to request bytes from a slave device.
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
 

  
 accXangle = (atan2(AcY, AcZ) * RAD_TO_DEG);
 accYangle = (atan2(AcX, AcZ) * RAD_TO_DEG);

 gyroXrate = GyX / 16.5;
 gyroYrate = GyY / 16.5;
 
 timer = millis();
 //angular position
 gyroXAngle += gyroXrate * (millis()-timer)/1000;
 gyroYAngle += gyroYrate * (millis()-timer)/1000;
 
 
 //---------------------------\\
 //COMPLIMENTRY FILTER STARTED\\
 //---------------------------\\

 compAngleX = ap * (compAngleX + gyroXAngle) + (1-ap) * accXangle;
 compAngleY = ap * (compAngleY + gyroYAngle) + (1-ap) * accYangle;
 
 //---------------------------\\
 //COMPLIMENTRY FILTER ENDED  \\
 //---------------------------\\



}



void pid()
{
  
 
  r0utput.Compute();

 
  p0utput.Compute();
}


void loop() {
   ////////////////Desired Pitch and roll/////////////
   if(CH3_delta_rl>1470 && CH3_delta_rl<1490)
 { droll=0;
}
    droll=map(CH3_delta_rl,1008,1968,-45,45);
      
droll=constrain(droll,-45,45);

          if(CH2_delta_pt>1460&&CH2_delta_pt<1490)
 { dpitch=0;
}

 dpitch=map(CH2_delta_pt,970,1976,-45,45);

 dpitch=constrain(dpitch,-45,45);
 
sensor_temp();



pid();



      


throttle=map(CH1_delta_th,988,1968,0,200);
throttle=constrain(throttle,0,200);

     




            if(roll<1.86 && roll>-1.86)
 { roll=0;
}     

            if(pitch<1.86&&pitch>-1.86)
 { pitch=0;
}


front_left=map(throttle+roll+pitch,0,170,0,200); // front left
front_right=map(throttle-roll+pitch,0,170,0,200);//front right
back_right=map(throttle-roll-pitch,0,170,0,200);//back right
back_left=map(throttle+roll-pitch,0,170,0,200);//back left

front_left=constrain(front_left,0,200); // front left
front_right=constrain(front_right,0,200);//front right
back_right=constrain(back_right,0,200);//back right
back_left=constrain(back_left,0,200);//back left

Serial.println("");
Serial.println("");
Serial.println("");
Serial.print("throttle Rcv");
Serial.println(CH1_delta_th);
Serial.print("Roll Rcv");
Serial.println(CH3_delta_rl);
Serial.print("Pitch Rcv");
Serial.println(CH2_delta_pt);
Serial.println("");
Serial.println("");
Serial.println("");
Serial.print("pitch ");
Serial.println(pitch);
Serial.print("roll ");
Serial.println(roll);
Serial.print("throttle ");
Serial.println(throttle); 
Serial.print("front_left ");
Serial.println(front_left);
Serial.print("front_right ");
Serial.println(front_right);
Serial.print("back_right ");
Serial.println(back_right);
Serial.print("back_left ");
Serial.println(back_left);

//======Writing Values====\\
analogWrite (esc3,front_left);//fl esc3
analogWrite (esc4,back_left);//bl esc4
analogWrite (esc1,front_right); //fr esc1
analogWrite (esc2,back_right);//br 

    /////////////////////////////////////////////
}
  analogWrite (esc3, front_left); //fl esc3
  analogWrite (esc4, back_left); //bl esc4
  analogWrite (esc1, front_right); //fr esc1
  analogWrite (esc2, back_right); //br

Forget about the PID for a moment. Do the ESCs behave as required when driven by a PWM signal from analogWrite() ? What PWM frequency will they accept ? It is more normal for them to be driven by outputs from the Servo library

I am very much new to the ESCs and BLDC. I think they kindda behave well by analogWrite(). I am using arduino mega 2560 and connected my ESC with pins of frequency of 490Hz. I have to calibrate my ESCs though when the program starts.
this is the program by which I am calibrating my ESC

#include <Servo.h>

#define MAX_SIGNAL 2100
#define MIN_SIGNAL 900

#define esc1 5//front right
#define esc2 6//back right 
#define esc3 7//front left 
#define esc4 8//back left

Servo firstESC,secESC,thirdESC,forthESC;

void setup() {
  Serial.begin(115200);
  Serial.println("Program begin...");
  Serial.println("This program will calibrate the ESC.");

  firstESC.attach(esc1);
  secESC.attach(esc2);
  thirdESC.attach(esc3);
  forthESC.attach(esc4);

  Serial.println("Now writing maximum output.");
  Serial.println("Turn on power source, then wait 2 seconds and press any key.");
  firstESC.writeMicroseconds(MAX_SIGNAL);
  secESC.writeMicroseconds(MAX_SIGNAL);
  thirdESC.writeMicroseconds(MAX_SIGNAL);
  forthESC.writeMicroseconds(MAX_SIGNAL);

  // Wait for input
  while (!Serial.available());
  Serial.read();

  // Send min output
  Serial.println("Sending minimum output");
  firstESC.writeMicroseconds(MIN_SIGNAL);
  secESC.writeMicroseconds(MIN_SIGNAL);
  thirdESC.writeMicroseconds(MIN_SIGNAL);
  forthESC.writeMicroseconds(MIN_SIGNAL);

}

void loop() {  

}

I have realized they kinda miss the speed when I increase my throttle. even if on full throttle, they are not giving their maximum thrust. Can you please tell me how can i drive my ESCs through servo library?? What changing should I have to make in my code? I am sure servo doesn’t accept the values ranging between 0-255 (PWM). Rather they would accept receiver output (in micro seconds).

ahsanraza: I am sure servo doesn't accept the values ranging between 0-255 (PWM). Rather they would accept receiver output (in micro seconds).

The servo library provides the correct timing for servos. It is not the same PWM as used for analogWrite().

aarg: The servo library provides the correct timing for servos. It is not the same PWM as used for analogWrite().

yeah I kinda wonder that. thanks for the post aarg. How can I convert my PWM signals to servo library signals? any help would be nie

ahsanraza: yeah I kinda wonder that. thanks for the post aarg. How can I convert my PWM signals to servo library signals? any help would be nie

Include the servo library and call the functions from it.

I'm not a "helicopter" person, but I have the impression that an ESC just translates servo signals into a motor drive voltage.

I have the impression that an ESC just translates servo signals into a motor drive voltage.

Close, but no cigar.

The ESCs used with brushless motors, which is the case here, translate the value from servo.write() or servo.writeMicroseconds() into a 3 phase supply to the motor to control the speed.

Essentially you can replace a servo with an ESC and instead of giving 0 to 180 degree servo movement you get zero to full speed out of the motor.

As to changing the program, include the Servo library, create 4 Servo objects, attach the 4 objects to Arduino pins (any digital pin will do, not just the PWM enabled ones) and use servo.write() or servo.writeMicroseconds() to drive the motors at the speed required.

The Servo library is included with the Arduino IDE and there are also examples supplied.

I have changed my code to this. Now I am driving ESC through servo library

#include<Wire.h>
const int MPU=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;//stores 
double compAngleX,compAngleY,timer;
  double accXangle ,accYangle ,gyroXrate ,gyroYrate;
  double gyroXAngle, gyroYAngle;
 
  int ap=0.955;

//------------------------------------------------------------------------------------------\\
////////////////// ESC CALIBRATION INT.//////////////////////////
#include <Servo.h>

#define MAX_SIGNAL 1990
#define MIN_SIGNAL 900

#define esc1 5//front right
#define esc2 6//back right 
#define esc3 9//front left 
#define esc4 8//back left

Servo firstESC,secESC,thirdESC,forthESC;
////////////////// ESC CALIBRATION END//////////////////////////


#include <Wire.h> 
#include <PID_v1.h>

//int escc1= 5;//front right
//int escc2= 6;//back right 
//int escc3= 9;//front left 
//int escc4= 8;//back left
double front_left,front_right,back_left,back_right;

double dpitch,droll;
double pitch,roll;
volatile double throttle;
float pid_p_gain_roll = 0.05;               //Gain setting for the roll P-controller (1.3)
float pid_i_gain_roll = 0;              //Gain setting for the roll I-controller (0.3)
float pid_d_gain_roll = 0;

float pid_p_gain_pitch = pid_p_gain_roll;  //Gain setting for the pitch P-controller.
float pid_i_gain_pitch = pid_i_gain_roll;  //Gain setting for the pitch I-controller.
float pid_d_gain_pitch = pid_d_gain_roll;  //Gain setting for the pitch D-controller.



#define PID_ROLL_INFLUENCE 30

/////////////////////////////////////////////////////////////


PID p0utput(&compAngleY,&pitch, &dpitch,pid_p_gain_pitch, pid_i_gain_pitch, pid_d_gain_pitch, DIRECT);
PID r0utput(&compAngleX,&roll, &droll, pid_p_gain_roll, pid_i_gain_roll, pid_d_gain_roll, DIRECT);
///////////////////////////////////////////////
/////////////////////////////////////////////////


////////////////////////PID DATA END//////////////
//--------------------------------------------------------------\\

////////////////////////RC_RX Init.///////////////////////////


#define CH1_int_th 0    // Channel 1 interrupt throttle 
#define CH1_pin_th 2     // Respective channel Hardware interrupt pin number

#define CH2_int_pt 1     // Channel 2 interrupt pitch 
#define CH2_pin_pt 3     // Respective channel Hardware interrupt pin number

#define CH3_int_r1 4     // Channel 3 interrupt roll 
#define CH3_pin_r1 19     // Respective channel Hardware interrupt pin number

#define CH4_int 5     // Channel 4 interrupt # 
#define CH4_pin 18    // Respective channel Hardware interrupt pin number
//#define CH5_pin 23
//#define CH6_pin 25

#define valid_pulse_limit 2000 // [uS] Valid output high pulse time limit for RC controller
#define max_high_time 1968 // [uS] Maximum expected high time
#define min_high_time 990 // [uS] Minimum expected high time
 

unsigned long t=0;
int counter;
void init_rc_rx();
//void read_rc_rx();  

volatile unsigned long CH1_t_th=0, CH1_delta_th=0, CH2_t_pt=0, CH2_delta_pt=0, CH3_t_rl=0, CH3_delta_rl=0 ;
//uncomment them for future use

volatile unsigned long CH4_t=0, CH4_delta=0;
//pulseIn channels for accessory 
//unsigned long CH5_delta=0,CH6_delta=0;

// Interrupt ISRs

void CH1_int_ISR()
{
  
  if ((micros()-CH1_t_th) < valid_pulse_limit){
    CH1_delta_th = micros()-CH1_t_th;
  }
  CH1_t_th = micros();
}

void CH2_int_ISR()
{
  
  if ((micros()-CH2_t_pt) < valid_pulse_limit){
    CH2_delta_pt = micros()-CH2_t_pt;
  }
  CH2_t_pt = micros();
}

void CH3_int_ISR()
{
  
  if ((micros()-CH3_t_rl) < valid_pulse_limit){
    CH3_delta_rl = micros()-CH3_t_rl;
  }
  CH3_t_rl = micros();
}

void CH4_int_ISR()
{
  
  if ((micros()-CH4_t) < valid_pulse_limit){
    CH4_delta = micros()-CH4_t;
  }
  CH4_t = micros();
  
}

// Initialization\\

void init_rc_rx(){
  

  //MAKING PINS INPUT//
  pinMode(CH1_pin_th, INPUT);
  pinMode(CH2_pin_pt, INPUT);
  pinMode(CH3_pin_r1, INPUT);
  pinMode(CH4_pin,INPUT);

  //ENABLING INTERRUPTS//
  attachInterrupt(CH1_int_th, CH1_int_ISR, CHANGE);
  attachInterrupt(CH2_int_pt, CH2_int_ISR, CHANGE);
  attachInterrupt(CH3_int_r1, CH3_int_ISR, CHANGE);
  attachInterrupt(CH4_int,CH4_int_ISR,CHANGE);

}


void sensor_temp() {
    Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,14);  // request a total of 14 registers//wire.requestFrom to request bytes from a slave device.
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
 

  
 accXangle = (atan2(AcY, AcZ) * RAD_TO_DEG);
 accYangle = (atan2(AcX, AcZ) * RAD_TO_DEG);

 gyroXrate = GyX / 16.5;
 gyroYrate = GyY / 16.5;
 
 timer = millis();
 //angular position
 gyroXAngle += gyroXrate * (millis()-timer)/1000;
 gyroYAngle += gyroYrate * (millis()-timer)/1000;
 

 compAngleX = ap * (compAngleX + gyroXAngle) + (1-ap) * accXangle;
 compAngleY = ap * (compAngleY + gyroYAngle) + (1-ap) * accYangle;




}


void pid()
{
  
 
  r0utput.Compute();

 
  p0utput.Compute();
}



//---------------------------------------------------------------------\\

void setup() {
  Serial.begin(115200);
    init_rc_rx();

   while (CH4_delta < 1800)
   {
     //do nothing
     Serial.println("Waiting...");
   }
     firstESC.attach(esc1);
  secESC.attach(esc2);
  thirdESC.attach(esc3);
  forthESC.attach(esc4);

  Serial.println("Now writing maximum output.");
  firstESC.writeMicroseconds(MAX_SIGNAL);
  secESC.writeMicroseconds(MAX_SIGNAL);
  thirdESC.writeMicroseconds(MAX_SIGNAL);
  forthESC.writeMicroseconds(MAX_SIGNAL);

 delay(3000);

  // Send min output
  Serial.println("Sending minimum output");
  firstESC.writeMicroseconds(MIN_SIGNAL);
  secESC.writeMicroseconds(MIN_SIGNAL);
  thirdESC.writeMicroseconds(MIN_SIGNAL);
  forthESC.writeMicroseconds(MIN_SIGNAL);
  delay(3000);
   


  
  
  
r0utput.SetMode(AUTOMATIC);//set pid on may be
r0utput.SetOutputLimits(-PID_ROLL_INFLUENCE, PID_ROLL_INFLUENCE);
  
p0utput.SetMode(AUTOMATIC);
p0utput.SetOutputLimits(-PID_ROLL_INFLUENCE, PID_ROLL_INFLUENCE);

  
//  pinMode(esc1,OUTPUT); 
//  pinMode(esc2,OUTPUT);
//  pinMode(esc3,OUTPUT); 
//  pinMode(esc4,OUTPUT);



// ================================================================
// ===                     SENSOR   SETUP                       ===
// ================================================================
 
/////////////////////// SENSOR READING//////////
 Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  ///////////////////////////////////////
  

}

void loop() {
  
   ////////////////Desired Pitch and roll/////////////
   if(CH3_delta_rl>1470) 
 { droll=CH3_delta_rl;
}
else if(CH3_delta_rl < 1492)
    droll=CH3_delta_rl;
      


          if(CH2_delta_pt>1460)
 { dpitch=CH2_delta_pt;
}
else if (CH2_delta_pt<1490)

 dpitch=CH2_delta_pt;


 sensor_temp();


pid();



      

throttle = CH1_delta_th;

     



front_left=(throttle+roll+pitch); // front left
front_right=(throttle-roll+pitch);//front right
back_right=(throttle-roll-pitch);//back right
back_left=(throttle+roll-pitch);//back left




firstESC.writeMicroseconds(front_right);
  secESC.writeMicroseconds(back_right);
  thirdESC.writeMicroseconds(front_left);
  forthESC.writeMicroseconds(back_left);

    /////////////////////////////////////////////
}

But now As the thread says, I am unable to tune my Quad . :frowning:

What do you expect pid() to actually do? It is going to modify the output argument of the pid object, dpitch and droll. Yet, these are inputs from the RC controller.

You don't understand PID, do you?

I just want to stabilize my quadcopter using MPU 6050. even if one of the motor is not working, my quad will balance itself without any difficulties. I somehow understand the working of PID but I am unable to find such gain values on which my quad will balance itself.

What input are you expecting the PID process to use? What goal is it trying to achieve? How are you planning to use the output value?

I somehow understand the working of PID

I don't think you do.

I have somehow manage to find the p_gain factor for roll and pitch. D and I gain factor are zero. the results are below

pitch 0.00 //output of PID roll -35.44/output of PID when my quad tilt to right or positive roll sense by sensor and no roll command sent./

throttle 1852.00 //us received by arduino

front_left 1781.13 // us calculated and written by writeMicroseconds front_right 1922.87//us calculated and written by writeMicroseconds back_right 1922.87//us calculated and written by writeMicroseconds back_left 1781.13 // us calculated and written by writeMicroseconds

All I wonder is that range of us enough ? I mean is the difference between the right and left sides of the motor is significant enough to balance a quad.

P.S. I have not attached props to my quad.