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@3:84d246dccb8d, 2015-02-19 (annotated)

Committer:
moklumbys
Date:
Thu Feb 19 23:16:00 2015 +0000
Revision:
3:84d246dccb8d
Parent:
2:62e387f1d095
Child:
8:0e9474cce85b
added comments.

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 0:341a08dbf9ba 10 }
moklumbys 0:341a08dbf9ba 11
moklumbys 3:84d246dccb8d 12 //------------------------------Function for ESC calibration---------------------
moklumbys 0:341a08dbf9ba 13 void Quadcopter::calibrate (float min, float max){
moklumbys 3:84d246dccb8d 14 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 3:84d246dccb8d 15 min_calibrate = max;
moklumbys 3:84d246dccb8d 16 max_calibrate = min;
moklumbys 3:84d246dccb8d 17 } else {
moklumbys 3:84d246dccb8d 18 min_calibrate = min;
moklumbys 3:84d246dccb8d 19 max_calibrate = max;
moklumbys 3:84d246dccb8d 20 }
moklumbys 3:84d246dccb8d 21 if ((min > 1.0) || (min < 0.0)) //here chech if values are in correct range. If they are not, make them to be in correct range
moklumbys 3:84d246dccb8d 22 min_calibrate = 0.0;
moklumbys 3:84d246dccb8d 23 if ((max > 1.0) || (max < 0.0))
moklumbys 3:84d246dccb8d 24 max_calibrate = 1.0;
moklumbys 3:84d246dccb8d 25
moklumbys 3:84d246dccb8d 26 for (int i = 0; i < 4; i++){ //run motors for some time in min speed
moklumbys 3:84d246dccb8d 27 *motor[i] = max_calibrate;
moklumbys 3:84d246dccb8d 28 }
moklumbys 3:84d246dccb8d 29 wait(6.0); //wait for the response from ESC modules
moklumbys 0:341a08dbf9ba 30 for (int i = 0; i < 4; i++){
moklumbys 3:84d246dccb8d 31 *motor[i] = min_calibrate; //run motors at maximum speed
moklumbys 0:341a08dbf9ba 32 }
moklumbys 3:84d246dccb8d 33 wait(2.0); //again wait for response
moklumbys 0:341a08dbf9ba 34 }
moklumbys 0:341a08dbf9ba 35 //-------------------------------------Function for Stabilising---------------
moklumbys 1:cadf589cab2f 36 void Quadcopter::stabilise (float* speed, float* actSpeed, float rollDiff, float pitchDiff){
moklumbys 3:84d246dccb8d 37 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 38 actSpeed[1] = speed[1] - (rollDiff / 2) + (pitchDiff / 2); //actual Speed is calculated as follows +- half rollDiff +- half pitchDiff
moklumbys 1:cadf589cab2f 39 actSpeed[2] = speed[2] + (rollDiff / 2) - (pitchDiff / 2);
moklumbys 1:cadf589cab2f 40 actSpeed[3] = speed[3] - (rollDiff / 2) - (pitchDiff / 2);
moklumbys 0:341a08dbf9ba 41 }
moklumbys 0:341a08dbf9ba 42 //-----------------------Function for producing thrust in Z direction --------
moklumbys 0:341a08dbf9ba 43 void Quadcopter::run (float* speed){
moklumbys 3:84d246dccb8d 44 //simply map values in the correct range and run PWM signals for each motor
moklumbys 0:341a08dbf9ba 45 for (int i = 0; i < 4; i++){
moklumbys 3:84d246dccb8d 46 *motor[i] = this->map(speed[i], 0.0, 1.0, min_calibrate, max_calibrate);
moklumbys 0:341a08dbf9ba 47 }
moklumbys 0:341a08dbf9ba 48 }
moklumbys 0:341a08dbf9ba 49 //-----------------------------Mapping function-----------------------------
moklumbys 3:84d246dccb8d 50 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 51 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
moklumbys 0:341a08dbf9ba 52 }