Diff: speedservo.cpp

Revision:

0:b583163ca5b3

Child:

1:331042d0fbff

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/speedservo.cpp	Fri Jan 18 03:57:28 2019 +0000
@@ -0,0 +1,68 @@
+#include "speedservo.h"
+#include "mbed.h"
+
+Servo::Servo(PinName Pin) : ServoPin(Pin) {}
+
+int Servo:: SpeedConvert (int _speed)
+{
+    Speed.detach();
+    if (_speed > 50)
+        _speed = 50;
+    else if (_speed < 1)
+        _speed =1;
+    speed = 10000/_speed;
+    Speed.attach_us(this, &Servo::Update, speed);
+    return speed;
+
+}
+
+void Servo::SetPosition(int Pos)             //servo go to position
+{
+    NewPosition = Pos;
+}
+
+void Servo::Update()
+{
+    if(NewPosition != CurrentPosition) {
+
+        if(NewPosition > CurrentPosition) {
+            CurrentPosition++;
+            Position =  CurrentPosition;
+
+        } else  {
+            CurrentPosition--;
+            Position = CurrentPosition;
+        }
+    }
+    Position = CurrentPosition;
+}
+
+
+void Servo::StartPulse()
+{
+    ServoPin = 1;
+
+    PulseStop.attach_us(this, &Servo::EndPulse, Position);
+}
+
+void Servo::EndPulse()
+{
+    ServoPin = 0;
+}
+
+void Servo::Enable(int StartPos, int RefreshRate, int _speed)
+{
+    SpeedConvert(_speed);
+    Period = (1000000/RefreshRate);                            //converts from HZ to period in  us
+    Position = StartPos;
+    NewPosition = StartPos;
+    CurrentPosition = StartPos;                              //sets CurrentPosition to  StartPos value
+    Pulse.attach_us(this, &Servo::StartPulse, Period);       //starts servo period Ticker r
+    //Speed.attach_us(this, &Servo::Update, speed);            //starts servo Speed Ticker  speed of servo update pulse width steps in  us,    slow =100000 100ms per step
+    SpeedConvert(_speed);
+}
+
+void Servo::Disable()           //turns off Ticker Pulse
+{
+    Pulse.detach();
+}
\ No newline at end of file