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@7:0c31ccba7b3c, 2015-02-25 (annotated)
- Committer:
- moklumbys
- Date:
- Wed Feb 25 10:38:30 2015 +0000
- Revision:
- 7:0c31ccba7b3c
- Parent:
- 6:5e815d4b4d8f
- Child:
- 8:56bdb0d7002b
enable calibrating by default.
Who changed what in which revision?
User | Revision | Line number | New 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 | 7:0c31ccba7b3c | 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 | } |