Servo.cpp

00001 #include "Servo.h"
00002 #include "mbed.h"
00003 
00004 Servo::Servo(PinName Pin) : ServoPin(Pin) {
00005     initialize(); // TODO: Works?
00006 }
00007 
00008 void Servo::initialize() {
00009     // initialize ESC
00010     Enable(1000,20000); // low throttle 50Hz TODO: Frequency modify
00011 }
00012 
00013 void Servo::SetPosition(int Pos) {
00014     Position = Pos;
00015 }
00016 
00017 void Servo::StartPulse() {
00018     ServoPin = 1;
00019     PulseStop.attach_us(this, &Servo::EndPulse, Position);
00020 }
00021 
00022 void Servo::EndPulse() {
00023     // my change
00024     PulseStop.detach();
00025     // my change
00026     ServoPin = 0;
00027 }
00028 
00029 void Servo::Enable(int StartPos, int Period) {
00030     Position = StartPos;
00031     Pulse.attach_us(this, &Servo::StartPulse, Period);
00032 }
00033 
00034 void Servo::Disable() {
00035     Pulse.detach();
00036 }
00037 
00038 void Servo::operator=(int position) {
00039     SetPosition(position);
00040 }