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.
}
}