Montvydas Klumbys / Mbed 2 deprecated QuadcopterProgram

Program used to control a quadcopter. It uses a PID library which can be found in: http://developer.mbed.org/cookbook/PID I also uses my own written library for easily controlling quadcopter motors, which can be found in: https://developer.mbed.org/users/moklumbys/code/Quadcopter/ One more library that I used is MPU6050 that was previously written by Erik Olieman but I was able to update it with new functions: https://developer.mbed.org/users/moklumbys/code/MPU6050/

Dependencies:   MPU6050 PID Quadcopter Servo mbed

main.cpp@6:5e815d4b4d8f, 2015-02-25 (annotated)

Committer:
moklumbys
Date:
Wed Feb 25 00:56:18 2015 +0000
Revision:
6:5e815d4b4d8f
Parent:
5:8b3f82abe3a4
Child:
7:0c31ccba7b3c
added bias values for PID controllers which corrected the problem with not giving negative (or sometimes positive) difference values.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
moklumbys 0:894ba50f267c 1 #include "mbed.h"
moklumbys 0:894ba50f267c 2 #include "Quadcopter.h"
moklumbys 1:40ade596b1e3 3 #include "PID.h"
moklumbys 1:40ade596b1e3 4 #include "MPU6050.h"
moklumbys 1:40ade596b1e3 5 #include "Timer.h"
moklumbys 1:40ade596b1e3 6
moklumbys 4:eb418af66d81 7 //defines the number of samples to be taken when calculating the offset for gyro and accelerometer
moklumbys 4:eb418af66d81 8 #define OFFSET_SAMPLES 50
moklumbys 1:40ade596b1e3 9
moklumbys 1:40ade596b1e3 10 //define how the accelerometer is placed on surface
moklumbys 1:40ade596b1e3 11 #define X_AXIS 1
moklumbys 1:40ade596b1e3 12 #define Y_AXIS 2
moklumbys 1:40ade596b1e3 13 #define Z_AXIS 0
moklumbys 0:894ba50f267c 14
moklumbys 4:eb418af66d81 15 //ESC calibration values
moklumbys 0:894ba50f267c 16 #define MAX_CALIBRATE 1.0
moklumbys 6:5e815d4b4d8f 17 #define MIN_CALIBRATE 0.0
moklumbys 0:894ba50f267c 18
moklumbys 4:eb418af66d81 19 //Just to remember which motor corresponds to which number...
moklumbys 0:894ba50f267c 20 #define FL 0 // Front left
moklumbys 0:894ba50f267c 21 #define FR 1 // Front right
moklumbys 0:894ba50f267c 22 #define BL 2 // back left
moklumbys 0:894ba50f267c 23 #define BR 3 // back right
moklumbys 1:40ade596b1e3 24
moklumbys 4:eb418af66d81 25 //input and output values for pitch
moklumbys 1:40ade596b1e3 26 #define PITCH_IN_MIN -90.0
moklumbys 1:40ade596b1e3 27 #define PITCH_IN_MAX 90.0
moklumbys 6:5e815d4b4d8f 28 #define PITCH_OUT_MIN -0.2
moklumbys 6:5e815d4b4d8f 29 #define PITCH_OUT_MAX 0.2
moklumbys 1:40ade596b1e3 30
moklumbys 4:eb418af66d81 31 //input and output values for roll
moklumbys 1:40ade596b1e3 32 #define ROLL_IN_MIN -90.0
moklumbys 1:40ade596b1e3 33 #define ROLL_IN_MAX 90.0
moklumbys 6:5e815d4b4d8f 34 #define ROLL_OUT_MIN -0.2
moklumbys 6:5e815d4b4d8f 35 #define ROLL_OUT_MAX 0.2
moklumbys 1:40ade596b1e3 36
moklumbys 4:eb418af66d81 37 //PID intervals/constants
moklumbys 6:5e815d4b4d8f 38 #define Kc 1.0
moklumbys 6:5e815d4b4d8f 39 #define Ti 0.00
moklumbys 3:5f43c8374ff2 40 #define Td 0.00
moklumbys 2:3161f535d71a 41 #define RATE 0.01
moklumbys 4:eb418af66d81 42
moklumbys 0:894ba50f267c 43 //--------------------------------ALL THE FUNCTION HEADERS-----------------------
moklumbys 4:eb418af66d81 44 float map(float x, float in_min, float in_max, float out_min, float out_max); //might be a useful function. One is used inside Quadcopter library though
moklumbys 0:894ba50f267c 45 //---------------------------------------END-------------------------------------
moklumbys 0:894ba50f267c 46
moklumbys 4:eb418af66d81 47 Quadcopter quad (p21, p22, p23, p24); //intance of the Quadcopter class
moklumbys 6:5e815d4b4d8f 48 Serial bt(p13, p14); //initialise a bluetooth module
moklumbys 4:eb418af66d81 49 Serial pc(USBTX, USBRX); // tx, rx
moklumbys 4:eb418af66d81 50 MPU6050 mpu(p9, p10); //Also disables sleep mode
moklumbys 4:eb418af66d81 51 Timer timer; //need a timer to tell how much time passed from the last calculation
moklumbys 1:40ade596b1e3 52
moklumbys 4:eb418af66d81 53 //put Kc, Ti, Td, interval for both pitch and roll PID models
moklumbys 1:40ade596b1e3 54 PID pitchPID (Kc, Ti, Td, RATE);
moklumbys 1:40ade596b1e3 55 PID rollPID (Kc, Ti, Td, RATE);
moklumbys 0:894ba50f267c 56
moklumbys 2:3161f535d71a 57 //***************************************STARTING MAIN FUNCTION*********************
moklumbys 0:894ba50f267c 58 int main() {
moklumbys 6:5e815d4b4d8f 59 bt.baud(9600);
moklumbys 4:eb418af66d81 60 pc.baud (115200); //fast transmition speed...
moklumbys 1:40ade596b1e3 61
moklumbys 6:5e815d4b4d8f 62 float val = 0.0;
moklumbys 6:5e815d4b4d8f 63 char buff; //buffers when bad value received
moklumbys 6:5e815d4b4d8f 64
moklumbys 4:eb418af66d81 65 float pitchDiff; //difference in pitch. Explained in PID library...
moklumbys 4:eb418af66d81 66 float rollDiff; //diference in roll
moklumbys 1:40ade596b1e3 67
moklumbys 4:eb418af66d81 68 float speed[4]; //speed for motors
moklumbys 4:eb418af66d81 69 float actSpeed[4]; //actual speed of for all motors
moklumbys 0:894ba50f267c 70
moklumbys 1:40ade596b1e3 71 float accOffset[3]; //offset values
moklumbys 1:40ade596b1e3 72 float gyroOffset[3];
moklumbys 1:40ade596b1e3 73 float angle[3]; //total calculated angle
moklumbys 1:40ade596b1e3 74
moklumbys 4:eb418af66d81 75 float prevTime; //previous time values will be given in the function
moklumbys 1:40ade596b1e3 76
moklumbys 1:40ade596b1e3 77 if (mpu.testConnection()) //just testing if things are working...
moklumbys 1:40ade596b1e3 78 pc.printf("MPU connection succeeded\n\r");
moklumbys 1:40ade596b1e3 79 else
moklumbys 1:40ade596b1e3 80 pc.printf("MPU connection failed\n\r");
moklumbys 1:40ade596b1e3 81
moklumbys 1:40ade596b1e3 82 mpu.setAlpha(0.97); //set Alpha coefficient for low/high pass filters
moklumbys 1:40ade596b1e3 83
moklumbys 6:5e815d4b4d8f 84 quad.setLimits(MIN_CALIBRATE, MAX_CALIBRATE); //set calibration limits for the system
moklumbys 6:5e815d4b4d8f 85 // quad.calibrate(); //calibrating motors
moklumbys 1:40ade596b1e3 86
moklumbys 1:40ade596b1e3 87 pitchPID.setInputLimits (PITCH_IN_MIN, PITCH_IN_MAX); //seting input and output limits for both pitch and roll
moklumbys 1:40ade596b1e3 88 pitchPID.setOutputLimits (PITCH_OUT_MIN, PITCH_OUT_MAX);
moklumbys 1:40ade596b1e3 89
moklumbys 1:40ade596b1e3 90 rollPID.setInputLimits (ROLL_IN_MIN, ROLL_IN_MAX);
moklumbys 1:40ade596b1e3 91 rollPID.setOutputLimits (ROLL_OUT_MIN, ROLL_OUT_MAX);
moklumbys 1:40ade596b1e3 92
moklumbys 6:5e815d4b4d8f 93 pitchPID.setBias(0.0); //need to define middle point!!!! very important!!!
moklumbys 6:5e815d4b4d8f 94 rollPID.setBias(0.0);
moklumbys 6:5e815d4b4d8f 95
moklumbys 1:40ade596b1e3 96 pitchPID.setMode(AUTO_MODE); //start stabilising by puting AUTO mode
moklumbys 1:40ade596b1e3 97 rollPID.setMode(AUTO_MODE);
moklumbys 1:40ade596b1e3 98
moklumbys 2:3161f535d71a 99 //need to vary this one to move quadcopter
moklumbys 4:eb418af66d81 100 pitchPID.setSetPoint (0.0); //seting the middle point meaning that quadcopter is balancing in one place
moklumbys 4:eb418af66d81 101 rollPID.setSetPoint (0.0);
moklumbys 1:40ade596b1e3 102
moklumbys 1:40ade596b1e3 103 mpu.getOffset(accOffset, gyroOffset, OFFSET_SAMPLES); //take some samples at the beginning to get an offset
moklumbys 4:eb418af66d81 104 wait(0.1); //wait to settle down
moklumbys 1:40ade596b1e3 105
moklumbys 4:eb418af66d81 106 timer.start(); //will need timer to detect when was the last time the values were updated
moklumbys 4:eb418af66d81 107 prevTime = timer.read(); //set previous timer value
moklumbys 2:3161f535d71a 108 //-------------------------------------------START INFINITE LOOP-------------------------------------------------
moklumbys 0:894ba50f267c 109 while(1) {
moklumbys 6:5e815d4b4d8f 110 if(bt.readable()){
moklumbys 6:5e815d4b4d8f 111 if (!bt.scanf("%f", &val)){
moklumbys 6:5e815d4b4d8f 112 bt.scanf("%c", &buff);
moklumbys 6:5e815d4b4d8f 113 } else {
moklumbys 6:5e815d4b4d8f 114 pitchPID.setTunings(val, 0.0, 0.0);
moklumbys 6:5e815d4b4d8f 115 rollPID.setTunings(val, 0.0, 0.0);
moklumbys 6:5e815d4b4d8f 116 pc.printf ("______________________SET______________________\n");
moklumbys 6:5e815d4b4d8f 117 }
moklumbys 6:5e815d4b4d8f 118 // if (buff == 'k'){
moklumbys 6:5e815d4b4d8f 119 // bt.scanf("%f", &val);
moklumbys 6:5e815d4b4d8f 120 // pitchPID.setTunings(val, 0.0, 0.0);
moklumbys 6:5e815d4b4d8f 121 // rollPID.setTunings(val, 0.0, 0.0);
moklumbys 6:5e815d4b4d8f 122 // }
moklumbys 6:5e815d4b4d8f 123 // if (buff == 's'){
moklumbys 6:5e815d4b4d8f 124 // bt.scanf("%f", &val);
moklumbys 6:5e815d4b4d8f 125 // spd = val;
moklumbys 6:5e815d4b4d8f 126 // }
moklumbys 6:5e815d4b4d8f 127 // }
moklumbys 6:5e815d4b4d8f 128 }
moklumbys 6:5e815d4b4d8f 129
moklumbys 6:5e815d4b4d8f 130 for (int i = 0; i < 4; i++){ //initialise speed to be 0.0
moklumbys 6:5e815d4b4d8f 131 speed[i] = 0.3;
moklumbys 6:5e815d4b4d8f 132 }
moklumbys 6:5e815d4b4d8f 133 quad.run (speed);
moklumbys 6:5e815d4b4d8f 134
moklumbys 5:8b3f82abe3a4 135 mpu.computeAngle (angle, accOffset, gyroOffset, timer.read()-prevTime); // get angle using all these values
moklumbys 2:3161f535d71a 136
moklumbys 2:3161f535d71a 137 rollPID.setInterval(timer.read()-prevTime); //need to change the interval because don't know how much time passed
moklumbys 2:3161f535d71a 138 pitchPID.setInterval(timer.read()-prevTime);
moklumbys 2:3161f535d71a 139
moklumbys 2:3161f535d71a 140 prevTime = timer.read(); //get present time -> will be used later on as previous value
moklumbys 0:894ba50f267c 141
moklumbys 6:5e815d4b4d8f 142 rollPID.setProcessValue (angle[Y_AXIS]); //take angle values, which correspond to pitch and roll and do processing
moklumbys 6:5e815d4b4d8f 143 pitchPID.setProcessValue (angle[X_AXIS]);
moklumbys 1:40ade596b1e3 144
moklumbys 2:3161f535d71a 145 pitchDiff = pitchPID.compute(); //compute the difference
moklumbys 1:40ade596b1e3 146 rollDiff = rollPID.compute();
moklumbys 4:eb418af66d81 147
moklumbys 6:5e815d4b4d8f 148 // pc.printf ("pitchDiff=%0.4f, rollDiff=%0.4f\n", pitchDiff, rollDiff);
moklumbys 4:eb418af66d81 149 quad.stabilise(speed, actSpeed, rollDiff, pitchDiff); //stabilise the speed by giving out actual Speed value
moklumbys 1:40ade596b1e3 150
moklumbys 2:3161f535d71a 151 //print some values to check how thing work out
moklumbys 6:5e815d4b4d8f 152 // pc.printf("x=%0.3f y=%0.3f z=%0.3f\n", angle[X_AXIS]+90, angle[Y_AXIS]+90, angle[Z_AXIS]);
moklumbys 3:5f43c8374ff2 153 // pc.printf("Speed_FL=%0.4f, Speed_FR=%0.4f, Speed_BL= %0.4f, Speed_BR=%0.4f\n", speed[FL], speed[FR], speed[BL], speed[BR]);
moklumbys 6:5e815d4b4d8f 154 pc.printf("ActSpeed_FL=%0.4f, ActSpeed_FR=%0.4f, ActSpeed_BL=%0.4f, ActSpeed_BR=%0.4f\n", actSpeed[FL], actSpeed[FR], actSpeed[BL], actSpeed[BR]);
moklumbys 4:eb418af66d81 155 quad.run(actSpeed); //run the motors at the spesified speed actSpeed
moklumbys 0:894ba50f267c 156 wait (0.01);
moklumbys 0:894ba50f267c 157 }
moklumbys 0:894ba50f267c 158 }
moklumbys 2:3161f535d71a 159 //************************************************END MAIN FUNCTION********************************************************
moklumbys 0:894ba50f267c 160
moklumbys 0:894ba50f267c 161 //-----------------------------Mapping function-----------------------------
moklumbys 1:40ade596b1e3 162 float map(float x, float in_min, float in_max, float out_min, float out_max){
moklumbys 0:894ba50f267c 163 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
moklumbys 0:894ba50f267c 164 }