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

Committer:
moklumbys
Date:
2015-02-18
Revision:
0:341a08dbf9ba
Child:
1:cadf589cab2f

File content as of revision 0:341a08dbf9ba:

#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;
}