Montvydas Klumbys
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
--- /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