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@2:62e387f1d095, 2015-02-19 (annotated)

Committer:
moklumbys
Date:
Thu Feb 19 12:19:17 2015 +0000
Revision:
2:62e387f1d095
Parent:
1:cadf589cab2f
Child:
3:84d246dccb8d
nothing much changed. Well, actually nothing :D

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 0:341a08dbf9ba 3 #include "Servo.h"
moklumbys 0:341a08dbf9ba 4
moklumbys 0:341a08dbf9ba 5
moklumbys 0:341a08dbf9ba 6 Quadcopter::Quadcopter(PinName FL, PinName FR, PinName BL, PinName BR){
moklumbys 0:341a08dbf9ba 7 motor[0] = new Servo (FL);
moklumbys 0:341a08dbf9ba 8 motor[1] = new Servo (FR);
moklumbys 0:341a08dbf9ba 9 motor[2] = new Servo (BL);
moklumbys 0:341a08dbf9ba 10 motor[3] = new Servo (BR);
moklumbys 0:341a08dbf9ba 11 }
moklumbys 0:341a08dbf9ba 12
moklumbys 0:341a08dbf9ba 13 //------------------------------Function for calibration---------------------
moklumbys 0:341a08dbf9ba 14 void Quadcopter::calibrate (float min, float max){
moklumbys 0:341a08dbf9ba 15 MIN_CALIBRATE = min;
moklumbys 0:341a08dbf9ba 16 MAX_CALIBRATE = max;
moklumbys 0:341a08dbf9ba 17
moklumbys 0:341a08dbf9ba 18 for (int i = 0; i < 4; i++){
moklumbys 0:341a08dbf9ba 19 *motor[i] = max;
moklumbys 0:341a08dbf9ba 20 }
moklumbys 0:341a08dbf9ba 21 wait(6.0);
moklumbys 0:341a08dbf9ba 22 for (int i = 0; i < 4; i++){
moklumbys 0:341a08dbf9ba 23 *motor[i] = min;
moklumbys 0:341a08dbf9ba 24 }
moklumbys 0:341a08dbf9ba 25 wait(2.0);
moklumbys 0:341a08dbf9ba 26 }
moklumbys 0:341a08dbf9ba 27 //-------------------------------------Function for Stabilising---------------
moklumbys 1:cadf589cab2f 28 void Quadcopter::stabilise (float* speed, float* actSpeed, float rollDiff, float pitchDiff){
moklumbys 1:cadf589cab2f 29 actSpeed[0] = speed[0] + (rollDiff / 2) + (pitchDiff / 2);
moklumbys 1:cadf589cab2f 30 actSpeed[1] = speed[1] - (rollDiff / 2) + (pitchDiff / 2);
moklumbys 1:cadf589cab2f 31 actSpeed[2] = speed[2] + (rollDiff / 2) - (pitchDiff / 2);
moklumbys 1:cadf589cab2f 32 actSpeed[3] = speed[3] - (rollDiff / 2) - (pitchDiff / 2);
moklumbys 2:62e387f1d095 33
moklumbys 2:62e387f1d095 34 // actSpeed[0] = speed[0] + (rollDiff / 2) + (pitchDiff / 2);
moklumbys 2:62e387f1d095 35 // actSpeed[1] = speed[1] - (rollDiff / 2) + (pitchDiff / 2);
moklumbys 2:62e387f1d095 36 // actSpeed[2] = speed[2] + (rollDiff / 2) - (pitchDiff / 2);
moklumbys 2:62e387f1d095 37 // actSpeed[3] = speed[3] - (rollDiff / 2) - (pitchDiff / 2);
moklumbys 0:341a08dbf9ba 38 }
moklumbys 0:341a08dbf9ba 39 //-----------------------Function for producing thrust in Z direction --------
moklumbys 0:341a08dbf9ba 40 void Quadcopter::run (float* speed){
moklumbys 0:341a08dbf9ba 41
moklumbys 0:341a08dbf9ba 42 for (int i = 0; i < 4; i++){
moklumbys 0:341a08dbf9ba 43 *motor[i] = this->map(speed[i], 0.0, 1.0, MIN_CALIBRATE, MAX_CALIBRATE);
moklumbys 0:341a08dbf9ba 44 }
moklumbys 0:341a08dbf9ba 45 }
moklumbys 0:341a08dbf9ba 46 //-----------------------------Mapping function-----------------------------
moklumbys 0:341a08dbf9ba 47 float Quadcopter::map(float x, float in_min, float in_max, float out_min, float out_max){
moklumbys 0:341a08dbf9ba 48 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
moklumbys 0:341a08dbf9ba 49 }