Montvydas Klumbys / Quadcopter

A library used for controlling a quadcopter. It provides an easy to use interface, which allows to mount, calibrate and run motors. It is also able to calibrate the actual speed according to calculated PID roll & pitch difference. I used the original Servo library as ESC modules use the same PWM signal as Servo motors.

Dependents:   QuadcopterProgram

Quadcopter.cpp@9:22c52af13ac2, 2015-02-25 (annotated)

Committer:
moklumbys
Date:
Wed Feb 25 00:55:08 2015 +0000
Revision:
9:22c52af13ac2
Parent:
8:0e9474cce85b
Child:
10:8147131fcebd
corrected min & max calibration values - now they are initialised as previously if someone didn't use calibrate at the beginning, then mapping was done from 0.0 to 0.0... which is obviously stupid.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
moklumbys 0:341a08dbf9ba 1 #include "Quadcopter.h"
moklumbys 0:341a08dbf9ba 2 #include "mbed.h"
moklumbys 3:84d246dccb8d 3 #include "Servo.h"
moklumbys 0:341a08dbf9ba 4
moklumbys 3:84d246dccb8d 5 Quadcopter::Quadcopter(PinName FL, PinName FR, PinName BL, PinName BR){ //we have 4 motors to control
moklumbys 3:84d246dccb8d 6 motor[0] = new Servo (FL); //motors are of class Servo as ESC are used in the similar manner
moklumbys 3:84d246dccb8d 7 motor[1] = new Servo (FR);
moklumbys 0:341a08dbf9ba 8 motor[2] = new Servo (BL);
moklumbys 0:341a08dbf9ba 9 motor[3] = new Servo (BR);
moklumbys 9:22c52af13ac2 10
moklumbys 9:22c52af13ac2 11 min_calibrate = 0.0;
moklumbys 9:22c52af13ac2 12 max_calibrate = 1.0;
moklumbys 0:341a08dbf9ba 13 }
moklumbys 0:341a08dbf9ba 14
moklumbys 3:84d246dccb8d 15 //------------------------------Function for ESC calibration---------------------
moklumbys 9:22c52af13ac2 16 void Quadcopter::calibrate (){
moklumbys 3:84d246dccb8d 17 for (int i = 0; i < 4; i++){ //run motors for some time in min speed
moklumbys 3:84d246dccb8d 18 *motor[i] = max_calibrate;
moklumbys 3:84d246dccb8d 19 }
moklumbys 3:84d246dccb8d 20 wait(6.0); //wait for the response from ESC modules
moklumbys 0:341a08dbf9ba 21 for (int i = 0; i < 4; i++){
moklumbys 3:84d246dccb8d 22 *motor[i] = min_calibrate; //run motors at maximum speed
moklumbys 9:22c52af13ac2 23 }
moklumbys 3:84d246dccb8d 24 wait(2.0); //again wait for response
moklumbys 0:341a08dbf9ba 25 }
moklumbys 0:341a08dbf9ba 26 //-------------------------------------Function for Stabilising---------------
moklumbys 1:cadf589cab2f 27 void Quadcopter::stabilise (float* speed, float* actSpeed, float rollDiff, float pitchDiff){
moklumbys 3:84d246dccb8d 28 actSpeed[0] = speed[0] + (rollDiff / 2) + (pitchDiff / 2); //each motor has actual Speed and speed at which we want them to fly...
moklumbys 3:84d246dccb8d 29 actSpeed[1] = speed[1] - (rollDiff / 2) + (pitchDiff / 2); //actual Speed is calculated as follows +- half rollDiff +- half pitchDiff
moklumbys 1:cadf589cab2f 30 actSpeed[2] = speed[2] + (rollDiff / 2) - (pitchDiff / 2);
moklumbys 1:cadf589cab2f 31 actSpeed[3] = speed[3] - (rollDiff / 2) - (pitchDiff / 2);
moklumbys 0:341a08dbf9ba 32 }
moklumbys 0:341a08dbf9ba 33 //-----------------------Function for producing thrust in Z direction --------
moklumbys 0:341a08dbf9ba 34 void Quadcopter::run (float* speed){
moklumbys 3:84d246dccb8d 35 //simply map values in the correct range and run PWM signals for each motor
moklumbys 0:341a08dbf9ba 36 for (int i = 0; i < 4; i++){
moklumbys 8:0e9474cce85b 37 if (speed[i] < 0.0)
moklumbys 9:22c52af13ac2 38 *motor[i] = min_calibrate;
moklumbys 8:0e9474cce85b 39 else if (speed[i] > 1.0)
moklumbys 8:0e9474cce85b 40 *motor[i] = max_calibrate;
moklumbys 8:0e9474cce85b 41 else
moklumbys 8:0e9474cce85b 42 *motor[i] = this->map(speed[i], 0.0, 1.0, min_calibrate, max_calibrate);
moklumbys 0:341a08dbf9ba 43 }
moklumbys 0:341a08dbf9ba 44 }
moklumbys 9:22c52af13ac2 45 //--------------------------Function for setting calibration limits-----------
moklumbys 9:22c52af13ac2 46 void Quadcopter::setLimits(float min, float max){
moklumbys 9:22c52af13ac2 47 if (min > max){ //here detect if someone tried making min to be more than max. If that is the case, then flip them together
moklumbys 9:22c52af13ac2 48 min_calibrate = max;
moklumbys 9:22c52af13ac2 49 max_calibrate = min;
moklumbys 9:22c52af13ac2 50 } else {
moklumbys 9:22c52af13ac2 51 min_calibrate = min;
moklumbys 9:22c52af13ac2 52 max_calibrate = max;
moklumbys 9:22c52af13ac2 53 }
moklumbys 9:22c52af13ac2 54 if ((min_calibrate > 1.0) || (min_calibrate < 0.0)) //here chech if values are in correct range. If they are not, make them to be in correct range
moklumbys 9:22c52af13ac2 55 min_calibrate = 0.0;
moklumbys 9:22c52af13ac2 56 if ((max_calibrate > 1.0) || (max_calibrate < 0.0))
moklumbys 9:22c52af13ac2 57 max_calibrate = 1.0;
moklumbys 9:22c52af13ac2 58 }
moklumbys 0:341a08dbf9ba 59 //-----------------------------Mapping function-----------------------------
moklumbys 3:84d246dccb8d 60 float Quadcopter::map(float x, float in_min, float in_max, float out_min, float out_max){ //simply maps values in the given range
moklumbys 0:341a08dbf9ba 61 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
moklumbys 9:22c52af13ac2 62 }
moklumbys 9:22c52af13ac2 63
moklumbys 9:22c52af13ac2 64 float Quadcopter::getLowerLimit(){
moklumbys 9:22c52af13ac2 65 return min_calibrate;
moklumbys 9:22c52af13ac2 66 }
moklumbys 9:22c52af13ac2 67
moklumbys 9:22c52af13ac2 68 float Quadcopter::getUpperLimit(){
moklumbys 9:22c52af13ac2 69 return max_calibrate;
moklumbys 9:22c52af13ac2 70
moklumbys 0:341a08dbf9ba 71 }