Diff: ServoMotorControl.cpp

Revision:

0:22a971e060ed

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ServoMotorControl.cpp	Sun Sep 24 18:41:24 2017 +0000
@@ -0,0 +1,65 @@
+#include "mbed.h"
+#include "ServoMotorControl.h"
+
+static float clamp(float value, float min, float max) {
+    if(value < min) {
+        return min;
+    } else if(value > max) {
+        return max;
+    } else {
+        return value;
+    }
+}
+
+
+Servo::Servo(PinName pwm) : _pwm(pwm)
+{
+    _pwm = 0;
+}
+
+void Servo::turn2Angle(int degree)
+{
+    float angle2value;
+    switch (degree){
+    case 0: angle2value = 1; break;
+    case 15: angle2value = 1.25; break;
+    case 30: angle2value = 1.5; break;
+    case 45: angle2value = 1.75; break;
+    case 60: angle2value = 2; break;
+    case 75: angle2value = 2.25; break;
+    case 90: angle2value = 2.5; break;
+    case 105: angle2value = 2.75; break;
+    case 120: angle2value = 3; break;
+    case 135: angle2value = 3.25; break;
+    case 150: angle2value = 3.5; break;
+    case 165: angle2value = 3.75; break;
+    case 180: angle2value = 4; break;
+    default: angle2value = 1; break;}
+    turn2pulse(angle2value);
+    
+}
+
+void Servo::turn2pulse(float value)
+{
+    //0     1
+    //30    1.5
+    //60    2
+    //90    2.5
+    //120   3
+    //150   3.5
+    //180   4
+    if (value < 1)
+    {
+        value = 1;
+    }
+    else if (value > 4)
+    {
+        value = 4;
+    }
+    value = value / 1500;
+    _pwm.pulsewidth(value);
+}
+float Servo::positionRead()
+{
+    return _position;
+}
\ No newline at end of file