Diff: Servo.cpp

Revision:

0:80c1c369a6f4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.cpp	Fri Feb 15 08:12:50 2019 +0000
@@ -0,0 +1,36 @@
+#include "Servo.h"
+#include "mbed.h"
+
+Servo::Servo(PinName Pin) : ServoPin(Pin) {}
+
+void Servo::init(int pos,float speed){
+    update(speed);
+    currentPosition = pos;
+    newPosition = pos;
+    Speed.attach_us(this, &Servo::move, 5000);
+}
+
+void Servo::update(float speed){
+    if(speed <= 0.0){
+        currentPosition = newPosition;
+    }else if(speed > 1.0){
+        speed = 1.0;
+    }
+    _speed = speed * 5; 
+}
+
+void Servo::move(){
+    float dev = newPosition - currentPosition;
+    if( dev > 0 ){
+        currentPosition += _speed;
+    }else if( dev < 0 ){
+        currentPosition -= _speed;
+    }
+    int pulse = int(currentPosition);
+    ServoPin.pulsewidth_us(pulse);
+}
+
+void Servo::setPosition(int pos,float speed){
+    newPosition = pos;
+    update(speed);
+}