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.
Diff: Quadcopter.cpp
- Revision:
- 0:341a08dbf9ba
- Child:
- 1:cadf589cab2f
diff -r 000000000000 -r 341a08dbf9ba Quadcopter.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Quadcopter.cpp Wed Feb 18 23:44:04 2015 +0000 @@ -0,0 +1,44 @@ +#include "Quadcopter.h" +#include "mbed.h" +#include "Servo.h" + + +Quadcopter::Quadcopter(PinName FL, PinName FR, PinName BL, PinName BR){ + motor[0] = new Servo (FL); + motor[1] = new Servo (FR); + motor[2] = new Servo (BL); + motor[3] = new Servo (BR); +} + +//------------------------------Function for calibration--------------------- +void Quadcopter::calibrate (float min, float max){ + MIN_CALIBRATE = min; + MAX_CALIBRATE = max; + + for (int i = 0; i < 4; i++){ + *motor[i] = max; + } + wait(6.0); + for (int i = 0; i < 4; i++){ + *motor[i] = min; + } + wait(2.0); +} +//-------------------------------------Function for Stabilising--------------- +void Quadcoter::stabilise (float* speed, float* actSpeed, float rollDiff, float pitchDiff){ + actSpeed[0] = speed[0] + (rollDif / 2) + (pitchDif / 2); + actSpeed[1] = speed[1] - (rollDif / 2) + (pitchDif / 2); + actSpeed[2] = speed[2] + (rollDif / 2) - (pitchDif / 2); + actSpeed[3] = speed[3] - (rollDif / 2) - (pitchDif / 2); +} +//-----------------------Function for producing thrust in Z direction -------- +void Quadcopter::run (float* speed){ + + for (int i = 0; i < 4; i++){ + *motor[i] = this->map(speed[i], 0.0, 1.0, MIN_CALIBRATE, MAX_CALIBRATE); + } +} +//-----------------------------Mapping function----------------------------- +float Quadcopter::map(float x, float in_min, float in_max, float out_min, float out_max){ + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} \ No newline at end of file