Matthias Grob
/
FlyBed2
My fully self designed first stable working Quadrocopter Software.
Diff: Servo/Servo.cpp
- Revision:
- 5:06e978fd147a
diff -r b0a60b0b24a9 -r 06e978fd147a Servo/Servo.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo/Servo.cpp Sun Jul 06 05:51:25 2014 +0000 @@ -0,0 +1,44 @@ +#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); +} \ No newline at end of file