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@8:56bdb0d7002b, 2015-03-01 (annotated)

Committer:
moklumbys
Date:
Sun Mar 01 22:45:31 2015 +0000
Revision:
8:56bdb0d7002b
Parent:
7:0c31ccba7b3c
Child:
9:588b1618c710
connects to bluetooth at the beginning, allowing to skip calibration of motors every time quadcopter is turned on.

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