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.
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?
User | Revision | Line number | New 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 | } |