Servo.cpp

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