Diff: Servo/Servo.cpp

Revision:

5:06e978fd147a

--- /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