Matthias Grob / Mbed 2 deprecated FlyBed3

Successful acro and level mode now! Relying on MPU9250 as base sensor. I'm working continuously on tuning and features :) NEWEST VERSION ON: https://github.com/MaEtUgR/FlyBed (CODE 100% compatible/copyable)

Dependencies:   mbed

Servo/Servo.cpp

Committer:
maetugr
Date:
2015-09-08
Revision:
0:37f0c1e8fa66

File content as of revision 0:37f0c1e8fa66:

#include "Servo.h"
#include "mbed.h"

Servo::Servo(PinName Pin, int frequency) : ServoPin(Pin) {
    Enable(1000,1000000/frequency); // low throttle 50Hz TODO: Frequency modify
}

void Servo::SetFrequency(int frequency) {
    Pulse.detach();
    Pulse.attach_us(this, &Servo::StartPulse, 1/frequency);
}

void Servo::SetPosition(int Pos) {
    if (Pos > 1000)
        Pos = 1000;
    if (Pos < 0)
        Pos = 0;
    Position = Pos+1000;
}

void Servo::StartPulse() {
    ServoPin = 1;
    PulseStop.attach_us(this, &Servo::EndPulse, Position);
}

void Servo::EndPulse() {
    // my change
    PulseStop.detach();
    // my change
    ServoPin = 0;
}

void Servo::Enable(int StartPos, int Period) {
    Position = StartPos;
    Pulse.attach_us(this, &Servo::StartPulse, Period);
}

void Servo::Disable() {
    Pulse.detach();
}

void Servo::operator=(int position) {
    SetPosition(position);
}