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@10:8147131fcebd, 2015-02-25 (annotated)

Committer:
moklumbys
Date:
Wed Feb 25 10:42:36 2015 +0000
Revision:
10:8147131fcebd
Parent:
9:22c52af13ac2 
Added getters for lower & upper calibration limits.

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 10:8147131fcebd 63 //-------------------------------Functiuon for getting the lower calibration limit------------
moklumbys 9:22c52af13ac2 64 float Quadcopter::getLowerLimit(){
moklumbys 9:22c52af13ac2 65 return min_calibrate;
moklumbys 9:22c52af13ac2 66 }
moklumbys 10:8147131fcebd 67 //----------------------------Function for getting upper calibration limit--------------------
moklumbys 9:22c52af13ac2 68 float Quadcopter::getUpperLimit(){
moklumbys 9:22c52af13ac2 69 return max_calibrate;
moklumbys 9:22c52af13ac2 70
moklumbys 0:341a08dbf9ba 71 }