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@2:3161f535d71a, 2015-02-19 (annotated)
- Committer:
- moklumbys
- Date:
- Thu Feb 19 10:38:43 2015 +0000
- Revision:
- 2:3161f535d71a
- Parent:
- 1:40ade596b1e3
- Child:
- 3:5f43c8374ff2
most of the things done for the quadcopter to be in stabilised position.;
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 | 1:40ade596b1e3 | 7 | #define MAX_CALIBRATE 1.0 |
moklumbys | 1:40ade596b1e3 | 8 | #define MIN_CALIBRATE 0.35 |
moklumbys | 1:40ade596b1e3 | 9 | |
moklumbys | 1:40ade596b1e3 | 10 | #define FL 0 // Front left |
moklumbys | 1:40ade596b1e3 | 11 | #define FR 1 // Front right |
moklumbys | 1:40ade596b1e3 | 12 | #define BL 2 // back left |
moklumbys | 1:40ade596b1e3 | 13 | #define BR 3 // back right |
moklumbys | 1:40ade596b1e3 | 14 | |
moklumbys | 1:40ade596b1e3 | 15 | #define OFFSET_SAMPLES 50 |
moklumbys | 1:40ade596b1e3 | 16 | //define how the accelerometer is placed on surface |
moklumbys | 1:40ade596b1e3 | 17 | #define X_AXIS 1 |
moklumbys | 1:40ade596b1e3 | 18 | #define Y_AXIS 2 |
moklumbys | 1:40ade596b1e3 | 19 | #define Z_AXIS 0 |
moklumbys | 0:894ba50f267c | 20 | |
moklumbys | 0:894ba50f267c | 21 | #define MAX_CALIBRATE 1.0 |
moklumbys | 0:894ba50f267c | 22 | #define MIN_CALIBRATE 0.35 |
moklumbys | 0:894ba50f267c | 23 | |
moklumbys | 0:894ba50f267c | 24 | #define FL 0 // Front left |
moklumbys | 0:894ba50f267c | 25 | #define FR 1 // Front right |
moklumbys | 0:894ba50f267c | 26 | #define BL 2 // back left |
moklumbys | 0:894ba50f267c | 27 | #define BR 3 // back right |
moklumbys | 1:40ade596b1e3 | 28 | |
moklumbys | 1:40ade596b1e3 | 29 | #define PITCH_IN_MIN -90.0 |
moklumbys | 1:40ade596b1e3 | 30 | #define PITCH_IN_MAX 90.0 |
moklumbys | 2:3161f535d71a | 31 | #define PITCH_OUT_MIN -0.5 |
moklumbys | 2:3161f535d71a | 32 | #define PITCH_OUT_MAX 0.5 |
moklumbys | 1:40ade596b1e3 | 33 | |
moklumbys | 1:40ade596b1e3 | 34 | #define ROLL_IN_MIN -90.0 |
moklumbys | 1:40ade596b1e3 | 35 | #define ROLL_IN_MAX 90.0 |
moklumbys | 2:3161f535d71a | 36 | #define ROLL_OUT_MIN -0.5 |
moklumbys | 2:3161f535d71a | 37 | #define ROLL_OUT_MAX 0.5 |
moklumbys | 1:40ade596b1e3 | 38 | |
moklumbys | 1:40ade596b1e3 | 39 | #define Kc 0.1 |
moklumbys | 1:40ade596b1e3 | 40 | #define Ti 0.1 |
moklumbys | 1:40ade596b1e3 | 41 | #define Td 0.0 |
moklumbys | 2:3161f535d71a | 42 | #define RATE 0.01 |
moklumbys | 0:894ba50f267c | 43 | //--------------------------------ALL THE FUNCTION HEADERS----------------------- |
moklumbys | 0:894ba50f267c | 44 | float map(float x, float in_min, float in_max, float out_min, float out_max); |
moklumbys | 0:894ba50f267c | 45 | //---------------------------------------END------------------------------------- |
moklumbys | 0:894ba50f267c | 46 | |
moklumbys | 0:894ba50f267c | 47 | Quadcopter quad (p21, p22, p23, p24); |
moklumbys | 0:894ba50f267c | 48 | Serial pc(USBTX, USBRX); // tx, rx |
moklumbys | 1:40ade596b1e3 | 49 | MPU6050 mpu(p9, p10); //Also disables sleep mode |
moklumbys | 1:40ade596b1e3 | 50 | Timer timer; |
moklumbys | 1:40ade596b1e3 | 51 | |
moklumbys | 1:40ade596b1e3 | 52 | //Kc, Ti, Td, interval |
moklumbys | 1:40ade596b1e3 | 53 | PID pitchPID (Kc, Ti, Td, RATE); |
moklumbys | 1:40ade596b1e3 | 54 | PID rollPID (Kc, Ti, Td, RATE); |
moklumbys | 0:894ba50f267c | 55 | |
moklumbys | 2:3161f535d71a | 56 | //***************************************STARTING MAIN FUNCTION********************* |
moklumbys | 0:894ba50f267c | 57 | int main() { |
moklumbys | 1:40ade596b1e3 | 58 | pc.baud (115200); |
moklumbys | 1:40ade596b1e3 | 59 | |
moklumbys | 0:894ba50f267c | 60 | float pitchDiff; |
moklumbys | 0:894ba50f267c | 61 | float rollDiff; |
moklumbys | 1:40ade596b1e3 | 62 | |
moklumbys | 0:894ba50f267c | 63 | float speed[4]; |
moklumbys | 0:894ba50f267c | 64 | float actSpeed[4]; |
moklumbys | 0:894ba50f267c | 65 | |
moklumbys | 1:40ade596b1e3 | 66 | float accOffset[3]; //offset values |
moklumbys | 1:40ade596b1e3 | 67 | float gyroOffset[3]; |
moklumbys | 1:40ade596b1e3 | 68 | float angle[3]; //total calculated angle |
moklumbys | 1:40ade596b1e3 | 69 | |
moklumbys | 1:40ade596b1e3 | 70 | float currTime; |
moklumbys | 1:40ade596b1e3 | 71 | float prevTime; |
moklumbys | 1:40ade596b1e3 | 72 | |
moklumbys | 1:40ade596b1e3 | 73 | if (mpu.testConnection()) //just testing if things are working... |
moklumbys | 1:40ade596b1e3 | 74 | pc.printf("MPU connection succeeded\n\r"); |
moklumbys | 1:40ade596b1e3 | 75 | else |
moklumbys | 1:40ade596b1e3 | 76 | pc.printf("MPU connection failed\n\r"); |
moklumbys | 1:40ade596b1e3 | 77 | |
moklumbys | 1:40ade596b1e3 | 78 | mpu.setAlpha(0.97); //set Alpha coefficient for low/high pass filters |
moklumbys | 1:40ade596b1e3 | 79 | |
moklumbys | 1:40ade596b1e3 | 80 | quad.calibrate(MIN_CALIBRATE, MAX_CALIBRATE); //calibrating motors |
moklumbys | 1:40ade596b1e3 | 81 | |
moklumbys | 1:40ade596b1e3 | 82 | pitchPID.setInputLimits (PITCH_IN_MIN, PITCH_IN_MAX); //seting input and output limits for both pitch and roll |
moklumbys | 1:40ade596b1e3 | 83 | pitchPID.setOutputLimits (PITCH_OUT_MIN, PITCH_OUT_MAX); |
moklumbys | 1:40ade596b1e3 | 84 | |
moklumbys | 1:40ade596b1e3 | 85 | rollPID.setInputLimits (ROLL_IN_MIN, ROLL_IN_MAX); |
moklumbys | 1:40ade596b1e3 | 86 | rollPID.setOutputLimits (ROLL_OUT_MIN, ROLL_OUT_MAX); |
moklumbys | 1:40ade596b1e3 | 87 | |
moklumbys | 1:40ade596b1e3 | 88 | pitchPID.setMode(AUTO_MODE); //start stabilising by puting AUTO mode |
moklumbys | 1:40ade596b1e3 | 89 | rollPID.setMode(AUTO_MODE); |
moklumbys | 1:40ade596b1e3 | 90 | |
moklumbys | 2:3161f535d71a | 91 | //need to vary this one to move quadcopter |
moklumbys | 1:40ade596b1e3 | 92 | pitchPID.setSetPoint (0.0); //seting the middle point.. Or smth like that, need to look into it more |
moklumbys | 2:3161f535d71a | 93 | rollPID.setSetPoint (0.0); //meaning that quadcopter is flying at a constant place.. no turning, blah blah blah |
moklumbys | 1:40ade596b1e3 | 94 | |
moklumbys | 1:40ade596b1e3 | 95 | mpu.getOffset(accOffset, gyroOffset, OFFSET_SAMPLES); //take some samples at the beginning to get an offset |
moklumbys | 1:40ade596b1e3 | 96 | wait(0.1); //wait to settle down |
moklumbys | 1:40ade596b1e3 | 97 | |
moklumbys | 1:40ade596b1e3 | 98 | timer.start(); //will need timer to detect when was the last time the values were updated |
moklumbys | 1:40ade596b1e3 | 99 | prevTime = timer.read(); |
moklumbys | 1:40ade596b1e3 | 100 | |
moklumbys | 2:3161f535d71a | 101 | for (int i = 0; i < 4; i++){ |
moklumbys | 2:3161f535d71a | 102 | speed[i] = 0.0; |
moklumbys | 2:3161f535d71a | 103 | } |
moklumbys | 2:3161f535d71a | 104 | //-------------------------------------------START INFINITE LOOP------------------------------------------------- |
moklumbys | 0:894ba50f267c | 105 | while(1) { |
moklumbys | 1:40ade596b1e3 | 106 | // for (float height = 0.0; height < 0.5; height+=0.05){ |
moklumbys | 1:40ade596b1e3 | 107 | // for (int i = 0; i < 4; i++){ |
moklumbys | 1:40ade596b1e3 | 108 | // speed[i] = height; |
moklumbys | 1:40ade596b1e3 | 109 | // } |
moklumbys | 1:40ade596b1e3 | 110 | // quad.run (speed); |
moklumbys | 1:40ade596b1e3 | 111 | // wait(0.1); |
moklumbys | 1:40ade596b1e3 | 112 | // } |
moklumbys | 1:40ade596b1e3 | 113 | // for (uint16_t i = 0; i < 600; i++) |
moklumbys | 1:40ade596b1e3 | 114 | // { |
moklumbys | 2:3161f535d71a | 115 | currTime = timer.read(); //get present time |
moklumbys | 2:3161f535d71a | 116 | mpu.computeAngle (angle, accOffset, gyroOffset, &currTime, &prevTime); // get angle using all these values |
moklumbys | 2:3161f535d71a | 117 | |
moklumbys | 2:3161f535d71a | 118 | rollPID.setInterval(timer.read()-prevTime); //need to change the interval because don't know how much time passed |
moklumbys | 2:3161f535d71a | 119 | pitchPID.setInterval(timer.read()-prevTime); |
moklumbys | 2:3161f535d71a | 120 | |
moklumbys | 2:3161f535d71a | 121 | prevTime = timer.read(); //get present time -> will be used later on as previous value |
moklumbys | 0:894ba50f267c | 122 | |
moklumbys | 2:3161f535d71a | 123 | rollPID.setProcessValue (angle[X_AXIS]); //take some values to do processing |
moklumbys | 1:40ade596b1e3 | 124 | pitchPID.setProcessValue (angle[Y_AXIS]); |
moklumbys | 1:40ade596b1e3 | 125 | |
moklumbys | 2:3161f535d71a | 126 | pitchDiff = pitchPID.compute(); //compute the difference |
moklumbys | 1:40ade596b1e3 | 127 | rollDiff = rollPID.compute(); |
moklumbys | 0:894ba50f267c | 128 | |
moklumbys | 2:3161f535d71a | 129 | quad.stabilise(speed, actSpeed, rollDiff, pitchDiff); //stabilise the speed by giving new actSpeed value |
moklumbys | 1:40ade596b1e3 | 130 | |
moklumbys | 2:3161f535d71a | 131 | //print some values to check how thing work out |
moklumbys | 2:3161f535d71a | 132 | // pc.printf("x=%0.3f y=%0.3f z=%0.3f\n", angle[X_AXIS], angle[Y_AXIS], angle[Z_AXIS]); |
moklumbys | 1:40ade596b1e3 | 133 | 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 | 1:40ade596b1e3 | 134 | 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 | 1:40ade596b1e3 | 135 | |
moklumbys | 2:3161f535d71a | 136 | // quad.run(actSpeed); //run the motors at the spesified speed actSpeed |
moklumbys | 0:894ba50f267c | 137 | |
moklumbys | 0:894ba50f267c | 138 | wait (0.01); |
moklumbys | 1:40ade596b1e3 | 139 | // } |
moklumbys | 0:894ba50f267c | 140 | |
moklumbys | 1:40ade596b1e3 | 141 | // for (float height = 0.5; height > 0.0; height-=0.05){ |
moklumbys | 1:40ade596b1e3 | 142 | // for (int i = 0; i < 4; i++){ |
moklumbys | 1:40ade596b1e3 | 143 | // speed[i] = height; |
moklumbys | 1:40ade596b1e3 | 144 | // } |
moklumbys | 1:40ade596b1e3 | 145 | // quad.run (speed); |
moklumbys | 1:40ade596b1e3 | 146 | // wait(0.1); |
moklumbys | 1:40ade596b1e3 | 147 | // } |
moklumbys | 1:40ade596b1e3 | 148 | // wait(1); |
moklumbys | 0:894ba50f267c | 149 | } |
moklumbys | 0:894ba50f267c | 150 | } |
moklumbys | 2:3161f535d71a | 151 | //************************************************END MAIN FUNCTION******************************************************** |
moklumbys | 0:894ba50f267c | 152 | |
moklumbys | 0:894ba50f267c | 153 | //-----------------------------Mapping function----------------------------- |
moklumbys | 1:40ade596b1e3 | 154 | float map(float x, float in_min, float in_max, float out_min, float out_max){ |
moklumbys | 0:894ba50f267c | 155 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
moklumbys | 0:894ba50f267c | 156 | } |