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
- Committer:
- moklumbys
- Date:
- 2015-02-19
- Revision:
- 2:62e387f1d095
- Parent:
- 1:cadf589cab2f
- Child:
- 3:84d246dccb8d
File content as of revision 2:62e387f1d095:
#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 Quadcopter::stabilise (float* speed, float* actSpeed, float rollDiff, float pitchDiff){ actSpeed[0] = speed[0] + (rollDiff / 2) + (pitchDiff / 2); actSpeed[1] = speed[1] - (rollDiff / 2) + (pitchDiff / 2); actSpeed[2] = speed[2] + (rollDiff / 2) - (pitchDiff / 2); actSpeed[3] = speed[3] - (rollDiff / 2) - (pitchDiff / 2); // actSpeed[0] = speed[0] + (rollDiff / 2) + (pitchDiff / 2); // actSpeed[1] = speed[1] - (rollDiff / 2) + (pitchDiff / 2); // actSpeed[2] = speed[2] + (rollDiff / 2) - (pitchDiff / 2); // actSpeed[3] = speed[3] - (rollDiff / 2) - (pitchDiff / 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; }