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@9:588b1618c710, 2015-03-01 (annotated)

Committer:
moklumbys
Date:
Sun Mar 01 22:53:30 2015 +0000
Revision:
9:588b1618c710
Parent:
8:56bdb0d7002b 
finished with BT.

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 8:56bdb0d7002b 28 #define PITCH_OUT_MIN -0.1
moklumbys 8:56bdb0d7002b 29 #define PITCH_OUT_MAX 0.1
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 8:56bdb0d7002b 34 #define ROLL_OUT_MIN -0.1
moklumbys 8:56bdb0d7002b 35 #define ROLL_OUT_MAX 0.1
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 8:56bdb0d7002b 85
moklumbys 9:588b1618c710 86 pc.printf ("Send c to calibrate and s to skip calibration.\n");
moklumbys 8:56bdb0d7002b 87 while(1){
moklumbys 8:56bdb0d7002b 88 if (!bt.readable()){
moklumbys 8:56bdb0d7002b 89 if (!bt.scanf("%f", &val)){
moklumbys 8:56bdb0d7002b 90 bt.scanf("%c", &buff);
moklumbys 8:56bdb0d7002b 91 } else {
moklumbys 9:588b1618c710 92 if (val == 's'){
moklumbys 9:588b1618c710 93 pc.printf ("Skipping calibration.\n");
moklumbys 9:588b1618c710 94 break;
moklumbys 9:588b1618c710 95 }
moklumbys 8:56bdb0d7002b 96 if (val == 'c'){
moklumbys 8:56bdb0d7002b 97 pc.printf ("Calibrating motors.\n");
moklumbys 8:56bdb0d7002b 98 quad.calibrate(); //calibrating motors
moklumbys 9:588b1618c710 99 } else {
moklumbys 9:588b1618c710 100 pc.printf ("Are you sure that you sent c/s?\n");
moklumbys 8:56bdb0d7002b 101 }
moklumbys 8:56bdb0d7002b 102 }
moklumbys 8:56bdb0d7002b 103 }
moklumbys 8:56bdb0d7002b 104 }
moklumbys 1:40ade596b1e3 105
moklumbys 1:40ade596b1e3 106 pitchPID.setInputLimits (PITCH_IN_MIN, PITCH_IN_MAX); //seting input and output limits for both pitch and roll
moklumbys 1:40ade596b1e3 107 pitchPID.setOutputLimits (PITCH_OUT_MIN, PITCH_OUT_MAX);
moklumbys 1:40ade596b1e3 108
moklumbys 1:40ade596b1e3 109 rollPID.setInputLimits (ROLL_IN_MIN, ROLL_IN_MAX);
moklumbys 1:40ade596b1e3 110 rollPID.setOutputLimits (ROLL_OUT_MIN, ROLL_OUT_MAX);
moklumbys 1:40ade596b1e3 111
moklumbys 6:5e815d4b4d8f 112 pitchPID.setBias(0.0); //need to define middle point!!!! very important!!!
moklumbys 6:5e815d4b4d8f 113 rollPID.setBias(0.0);
moklumbys 6:5e815d4b4d8f 114
moklumbys 1:40ade596b1e3 115 pitchPID.setMode(AUTO_MODE); //start stabilising by puting AUTO mode
moklumbys 1:40ade596b1e3 116 rollPID.setMode(AUTO_MODE);
moklumbys 1:40ade596b1e3 117
moklumbys 2:3161f535d71a 118 //need to vary this one to move quadcopter
moklumbys 4:eb418af66d81 119 pitchPID.setSetPoint (0.0); //seting the middle point meaning that quadcopter is balancing in one place
moklumbys 4:eb418af66d81 120 rollPID.setSetPoint (0.0);
moklumbys 1:40ade596b1e3 121
moklumbys 1:40ade596b1e3 122 mpu.getOffset(accOffset, gyroOffset, OFFSET_SAMPLES); //take some samples at the beginning to get an offset
moklumbys 4:eb418af66d81 123 wait(0.1); //wait to settle down
moklumbys 1:40ade596b1e3 124
moklumbys 4:eb418af66d81 125 timer.start(); //will need timer to detect when was the last time the values were updated
moklumbys 4:eb418af66d81 126 prevTime = timer.read(); //set previous timer value
moklumbys 8:56bdb0d7002b 127
moklumbys 8:56bdb0d7002b 128 for (int i = 0; i < 4; i++){ //initialise speed to be 0.3
moklumbys 8:56bdb0d7002b 129 speed[i] = 0.2;
moklumbys 8:56bdb0d7002b 130 }
moklumbys 8:56bdb0d7002b 131 quad.run (speed);
moklumbys 2:3161f535d71a 132 //-------------------------------------------START INFINITE LOOP-------------------------------------------------
moklumbys 0:894ba50f267c 133 while(1) {
moklumbys 6:5e815d4b4d8f 134 if(bt.readable()){
moklumbys 6:5e815d4b4d8f 135 if (!bt.scanf("%f", &val)){
moklumbys 6:5e815d4b4d8f 136 bt.scanf("%c", &buff);
moklumbys 6:5e815d4b4d8f 137 } else {
moklumbys 6:5e815d4b4d8f 138 pitchPID.setTunings(val, 0.0, 0.0);
moklumbys 6:5e815d4b4d8f 139 rollPID.setTunings(val, 0.0, 0.0);
moklumbys 6:5e815d4b4d8f 140 pc.printf ("______________________SET______________________\n");
moklumbys 9:588b1618c710 141 }
moklumbys 9:588b1618c710 142 }
moklumbys 9:588b1618c710 143
moklumbys 5:8b3f82abe3a4 144 mpu.computeAngle (angle, accOffset, gyroOffset, timer.read()-prevTime); // get angle using all these values
moklumbys 2:3161f535d71a 145
moklumbys 2:3161f535d71a 146 rollPID.setInterval(timer.read()-prevTime); //need to change the interval because don't know how much time passed
moklumbys 2:3161f535d71a 147 pitchPID.setInterval(timer.read()-prevTime);
moklumbys 2:3161f535d71a 148
moklumbys 2:3161f535d71a 149 prevTime = timer.read(); //get present time -> will be used later on as previous value
moklumbys 0:894ba50f267c 150
moklumbys 6:5e815d4b4d8f 151 rollPID.setProcessValue (angle[Y_AXIS]); //take angle values, which correspond to pitch and roll and do processing
moklumbys 6:5e815d4b4d8f 152 pitchPID.setProcessValue (angle[X_AXIS]);
moklumbys 1:40ade596b1e3 153
moklumbys 2:3161f535d71a 154 pitchDiff = pitchPID.compute(); //compute the difference
moklumbys 1:40ade596b1e3 155 rollDiff = rollPID.compute();
moklumbys 4:eb418af66d81 156
moklumbys 6:5e815d4b4d8f 157 // pc.printf ("pitchDiff=%0.4f, rollDiff=%0.4f\n", pitchDiff, rollDiff);
moklumbys 4:eb418af66d81 158 quad.stabilise(speed, actSpeed, rollDiff, pitchDiff); //stabilise the speed by giving out actual Speed value
moklumbys 1:40ade596b1e3 159
moklumbys 2:3161f535d71a 160 //print some values to check how thing work out
moklumbys 6:5e815d4b4d8f 161 // 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 162 // 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 163 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 164 quad.run(actSpeed); //run the motors at the spesified speed actSpeed
moklumbys 0:894ba50f267c 165 wait (0.01);
moklumbys 0:894ba50f267c 166 }
moklumbys 0:894ba50f267c 167 }
moklumbys 2:3161f535d71a 168 //************************************************END MAIN FUNCTION********************************************************
moklumbys 0:894ba50f267c 169
moklumbys 0:894ba50f267c 170 //-----------------------------Mapping function-----------------------------
moklumbys 1:40ade596b1e3 171 float map(float x, float in_min, float in_max, float out_min, float out_max){
moklumbys 0:894ba50f267c 172 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
moklumbys 0:894ba50f267c 173 }