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