Montvydas Klumbys / Mbed 2 deprecated QuadcopterProgram

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MPU6050 PID Quadcopter Servo mbed

main.cpp@1:40ade596b1e3, 2015-02-19 (annotated)

Committer:
moklumbys
Date:
Thu Feb 19 00:16:33 2015 +0000
Revision:
1:40ade596b1e3
Parent:
0:894ba50f267c
Child:
2:3161f535d71a
some new features - printing values for speed, depending on the angle. Will be able to test it now using gyro&acc!;

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 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 1:40ade596b1e3 31 #define PITCH_OUT_MIN -0.1
moklumbys 1:40ade596b1e3 32 #define PITCH_OUT_MAX 0.1
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 1:40ade596b1e3 36 #define ROLL_OUT_MIN -0.1
moklumbys 1:40ade596b1e3 37 #define ROLL_OUT_MAX 0.1
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 1:40ade596b1e3 42 #define RATE 0.1
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 0:894ba50f267c 56 int main() {
moklumbys 1:40ade596b1e3 57 pc.baud (115200);
moklumbys 1:40ade596b1e3 58
moklumbys 0:894ba50f267c 59 float pitchDiff;
moklumbys 0:894ba50f267c 60 float rollDiff;
moklumbys 1:40ade596b1e3 61
moklumbys 0:894ba50f267c 62 float speed[4];
moklumbys 0:894ba50f267c 63 float actSpeed[4];
moklumbys 0:894ba50f267c 64
moklumbys 1:40ade596b1e3 65 float accOffset[3]; //offset values
moklumbys 1:40ade596b1e3 66 float gyroOffset[3];
moklumbys 1:40ade596b1e3 67 float angle[3]; //total calculated angle
moklumbys 1:40ade596b1e3 68
moklumbys 1:40ade596b1e3 69 float currTime;
moklumbys 1:40ade596b1e3 70 float prevTime;
moklumbys 1:40ade596b1e3 71
moklumbys 1:40ade596b1e3 72 if (mpu.testConnection()) //just testing if things are working...
moklumbys 1:40ade596b1e3 73 pc.printf("MPU connection succeeded\n\r");
moklumbys 1:40ade596b1e3 74 else
moklumbys 1:40ade596b1e3 75 pc.printf("MPU connection failed\n\r");
moklumbys 1:40ade596b1e3 76
moklumbys 1:40ade596b1e3 77 mpu.setAlpha(0.97); //set Alpha coefficient for low/high pass filters
moklumbys 1:40ade596b1e3 78
moklumbys 1:40ade596b1e3 79 quad.calibrate(MIN_CALIBRATE, MAX_CALIBRATE); //calibrating motors
moklumbys 1:40ade596b1e3 80
moklumbys 1:40ade596b1e3 81 pitchPID.setInputLimits (PITCH_IN_MIN, PITCH_IN_MAX); //seting input and output limits for both pitch and roll
moklumbys 1:40ade596b1e3 82 pitchPID.setOutputLimits (PITCH_OUT_MIN, PITCH_OUT_MAX);
moklumbys 1:40ade596b1e3 83
moklumbys 1:40ade596b1e3 84 rollPID.setInputLimits (ROLL_IN_MIN, ROLL_IN_MAX);
moklumbys 1:40ade596b1e3 85 rollPID.setOutputLimits (ROLL_OUT_MIN, ROLL_OUT_MAX);
moklumbys 1:40ade596b1e3 86
moklumbys 1:40ade596b1e3 87 pitchPID.setMode(AUTO_MODE); //start stabilising by puting AUTO mode
moklumbys 1:40ade596b1e3 88 rollPID.setMode(AUTO_MODE);
moklumbys 1:40ade596b1e3 89
moklumbys 1:40ade596b1e3 90 pitchPID.setSetPoint (0.0); //seting the middle point.. Or smth like that, need to look into it more
moklumbys 1:40ade596b1e3 91 rollPID.setSetPoint (0.0);
moklumbys 1:40ade596b1e3 92
moklumbys 1:40ade596b1e3 93 mpu.getOffset(accOffset, gyroOffset, OFFSET_SAMPLES); //take some samples at the beginning to get an offset
moklumbys 1:40ade596b1e3 94 wait(0.1); //wait to settle down
moklumbys 1:40ade596b1e3 95
moklumbys 1:40ade596b1e3 96 timer.start(); //will need timer to detect when was the last time the values were updated
moklumbys 1:40ade596b1e3 97 prevTime = timer.read();
moklumbys 1:40ade596b1e3 98
moklumbys 0:894ba50f267c 99 while(1) {
moklumbys 1:40ade596b1e3 100 // for (float height = 0.0; height < 0.5; height+=0.05){
moklumbys 1:40ade596b1e3 101 // for (int i = 0; i < 4; i++){
moklumbys 1:40ade596b1e3 102 // speed[i] = height;
moklumbys 1:40ade596b1e3 103 // }
moklumbys 1:40ade596b1e3 104 // quad.run (speed);
moklumbys 1:40ade596b1e3 105 // wait(0.1);
moklumbys 1:40ade596b1e3 106 // }
moklumbys 1:40ade596b1e3 107 // for (uint16_t i = 0; i < 600; i++)
moklumbys 1:40ade596b1e3 108 // {
moklumbys 1:40ade596b1e3 109 currTime = timer.read();
moklumbys 1:40ade596b1e3 110 mpu.computeAngle (angle, accOffset, gyroOffset, &currTime, &prevTime); // get angle
moklumbys 1:40ade596b1e3 111 prevTime = timer.read();
moklumbys 0:894ba50f267c 112
moklumbys 1:40ade596b1e3 113 rollPID.setProcessValue (angle[X_AXIS]);
moklumbys 1:40ade596b1e3 114 pitchPID.setProcessValue (angle[Y_AXIS]);
moklumbys 1:40ade596b1e3 115
moklumbys 1:40ade596b1e3 116 pitchDiff = pitchPID.compute();
moklumbys 1:40ade596b1e3 117 rollDiff = rollPID.compute();
moklumbys 0:894ba50f267c 118
moklumbys 0:894ba50f267c 119 quad.stabilise(speed, actSpeed, rollDiff, pitchDiff);
moklumbys 1:40ade596b1e3 120
moklumbys 1:40ade596b1e3 121 pc.printf("x=%0.3f y=%0.3f z=%0.3f\n", angle[X_AXIS], angle[Y_AXIS], angle[Z_AXIS]);
moklumbys 1:40ade596b1e3 122 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 123 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 124
moklumbys 0:894ba50f267c 125 quad.run(actSpeed);
moklumbys 0:894ba50f267c 126
moklumbys 0:894ba50f267c 127 wait (0.01);
moklumbys 1:40ade596b1e3 128 // }
moklumbys 0:894ba50f267c 129
moklumbys 1:40ade596b1e3 130 // for (float height = 0.5; height > 0.0; height-=0.05){
moklumbys 1:40ade596b1e3 131 // for (int i = 0; i < 4; i++){
moklumbys 1:40ade596b1e3 132 // speed[i] = height;
moklumbys 1:40ade596b1e3 133 // }
moklumbys 1:40ade596b1e3 134 // quad.run (speed);
moklumbys 1:40ade596b1e3 135 // wait(0.1);
moklumbys 1:40ade596b1e3 136 // }
moklumbys 1:40ade596b1e3 137 // wait(1);
moklumbys 0:894ba50f267c 138 }
moklumbys 0:894ba50f267c 139 }
moklumbys 0:894ba50f267c 140
moklumbys 0:894ba50f267c 141 //-----------------------------Mapping function-----------------------------
moklumbys 1:40ade596b1e3 142 float map(float x, float in_min, float in_max, float out_min, float out_max){
moklumbys 0:894ba50f267c 143 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
moklumbys 0:894ba50f267c 144 }