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@0:894ba50f267c, 2015-02-18 (annotated)

Committer:
moklumbys
Date:
Wed Feb 18 23:44:09 2015 +0000
Revision:
0:894ba50f267c
Child:
1:40ade596b1e3
First attempt, still did not include MPU6050, but started including something...

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 0:894ba50f267c 3
moklumbys 0:894ba50f267c 4 #define MAX_CALIBRATE 1.0
moklumbys 0:894ba50f267c 5 #define MIN_CALIBRATE 0.35
moklumbys 0:894ba50f267c 6
moklumbys 0:894ba50f267c 7 #define FL 0 // Front left
moklumbys 0:894ba50f267c 8 #define FR 1 // Front right
moklumbys 0:894ba50f267c 9 #define BL 2 // back left
moklumbys 0:894ba50f267c 10 #define BR 3 // back right
moklumbys 0:894ba50f267c 11 //--------------------------------ALL THE FUNCTION HEADERS-----------------------
moklumbys 0:894ba50f267c 12 float map(float x, float in_min, float in_max, float out_min, float out_max);
moklumbys 0:894ba50f267c 13 //---------------------------------------END-------------------------------------
moklumbys 0:894ba50f267c 14
moklumbys 0:894ba50f267c 15 Quadcopter quad (p21, p22, p23, p24);
moklumbys 0:894ba50f267c 16 Serial pc(USBTX, USBRX); // tx, rx
moklumbys 0:894ba50f267c 17
moklumbys 0:894ba50f267c 18 int main() {
moklumbys 0:894ba50f267c 19 float pitchDiff;
moklumbys 0:894ba50f267c 20 float rollDiff;
moklumbys 0:894ba50f267c 21 float speed[4];
moklumbys 0:894ba50f267c 22 float actSpeed[4];
moklumbys 0:894ba50f267c 23
moklumbys 0:894ba50f267c 24 quad.calibrate(MIN_CALIBRATE, MAX_CALIBRATE);
moklumbys 0:894ba50f267c 25 while(1) {
moklumbys 0:894ba50f267c 26 for (float height = 0.0; height < 0.5; height+=0.05){
moklumbys 0:894ba50f267c 27 for (int i = 0; i < 4; i++){
moklumbys 0:894ba50f267c 28 speed[i] = height;
moklumbys 0:894ba50f267c 29 }
moklumbys 0:894ba50f267c 30 quad.run (speed);
moklumbys 0:894ba50f267c 31 wait(0.1);
moklumbys 0:894ba50f267c 32 }
moklumbys 0:894ba50f267c 33 for (uint16_t i = 0; i < 600; i++)
moklumbys 0:894ba50f267c 34 {
moklumbys 0:894ba50f267c 35 rollPID.setProcessValue (imu.roll);
moklumbys 0:894ba50f267c 36 rollPID.setProcessValue (imu.pitch);
moklumbys 0:894ba50f267c 37
moklumbys 0:894ba50f267c 38 pitchDif = pitchPID.compute();
moklumbys 0:894ba50f267c 39 rollDif = rollPID.compute();
moklumbys 0:894ba50f267c 40
moklumbys 0:894ba50f267c 41 quad.stabilise(speed, actSpeed, rollDiff, pitchDiff);
moklumbys 0:894ba50f267c 42 quad.run(actSpeed);
moklumbys 0:894ba50f267c 43
moklumbys 0:894ba50f267c 44 wait (0.01);
moklumbys 0:894ba50f267c 45 }
moklumbys 0:894ba50f267c 46
moklumbys 0:894ba50f267c 47 for (float height = 0.5; height > 0.0; height-=0.05){
moklumbys 0:894ba50f267c 48 for (int i = 0; i < 4; i++){
moklumbys 0:894ba50f267c 49 speed[i] = height;
moklumbys 0:894ba50f267c 50 }
moklumbys 0:894ba50f267c 51 quad.run (speed);
moklumbys 0:894ba50f267c 52 wait(0.1);
moklumbys 0:894ba50f267c 53 }
moklumbys 0:894ba50f267c 54 wait(1);
moklumbys 0:894ba50f267c 55 }
moklumbys 0:894ba50f267c 56 }
moklumbys 0:894ba50f267c 57
moklumbys 0:894ba50f267c 58 //-----------------------------Mapping function-----------------------------
moklumbys 0:894ba50f267c 59 float map(float x, float in_min, float in_max, float out_min, float out_max)
moklumbys 0:894ba50f267c 60 {
moklumbys 0:894ba50f267c 61 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
moklumbys 0:894ba50f267c 62 }