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@0:341a08dbf9ba, 2015-02-18 (annotated)
- Committer:
- moklumbys
- Date:
- Wed Feb 18 23:44:04 2015 +0000
- Revision:
- 0:341a08dbf9ba
- Child:
- 1:cadf589cab2f
So I wrote a library for the quadcopter to work with it easier.
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 | 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 | 0:341a08dbf9ba | 28 | void Quadcoter::stabilise (float* speed, float* actSpeed, float rollDiff, float pitchDiff){ |
moklumbys | 0:341a08dbf9ba | 29 | actSpeed[0] = speed[0] + (rollDif / 2) + (pitchDif / 2); |
moklumbys | 0:341a08dbf9ba | 30 | actSpeed[1] = speed[1] - (rollDif / 2) + (pitchDif / 2); |
moklumbys | 0:341a08dbf9ba | 31 | actSpeed[2] = speed[2] + (rollDif / 2) - (pitchDif / 2); |
moklumbys | 0:341a08dbf9ba | 32 | actSpeed[3] = speed[3] - (rollDif / 2) - (pitchDif / 2); |
moklumbys | 0:341a08dbf9ba | 33 | } |
moklumbys | 0:341a08dbf9ba | 34 | //-----------------------Function for producing thrust in Z direction -------- |
moklumbys | 0:341a08dbf9ba | 35 | void Quadcopter::run (float* speed){ |
moklumbys | 0:341a08dbf9ba | 36 | |
moklumbys | 0:341a08dbf9ba | 37 | for (int i = 0; i < 4; i++){ |
moklumbys | 0:341a08dbf9ba | 38 | *motor[i] = this->map(speed[i], 0.0, 1.0, MIN_CALIBRATE, MAX_CALIBRATE); |
moklumbys | 0:341a08dbf9ba | 39 | } |
moklumbys | 0:341a08dbf9ba | 40 | } |
moklumbys | 0:341a08dbf9ba | 41 | //-----------------------------Mapping function----------------------------- |
moklumbys | 0:341a08dbf9ba | 42 | float Quadcopter::map(float x, float in_min, float in_max, float out_min, float out_max){ |
moklumbys | 0:341a08dbf9ba | 43 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
moklumbys | 0:341a08dbf9ba | 44 | } |