Diff: CYS8218Controller.cpp

Revision:

1:c42f4b73e85c

Parent:

0:23a6381d15ff

Child:

2:695c74c6d483

--- a/CYS8218Controller.cpp	Wed Sep 07 00:15:46 2016 +0000
+++ b/CYS8218Controller.cpp	Wed Sep 07 03:07:07 2016 +0200
@@ -0,0 +1,42 @@
+#include "CYS8218Controller.hpp"
+
+CYS8218Controller::CYS8218Controller(PinName pwmPin, float initAngle) :
+  _servo(pwmPin)
+{
+  _angle = initAngle;
+  ZEROPW = 0.0015f; //1500us default position
+  PW_PER_DEG = 0.0005f/45.0f; //500us -> 45deg default
+  _servo.period_us(5000);
+  SetServo();
+}
+
+void CYS8218Controller::SetServo()
+{
+  float pw = ZEROPW+_angle*PW_PER_DEG;
+
+  pw = ( pw < MIN_PW ? MIN_PW : pw > MAX_PW ? MAX_PW : pw);
+
+  _servo.pulsewidth(pw);
+}
+
+void CYS8218Controller::SaveZero()
+{
+  ZEROPW = (ZEROPW+_angle*PW_PER_DEG);
+}
+
+void CYS8218Controller::Calibrate(float actualAngle)
+{
+  PW_PER_DEG = (_angle*PW_PER_DEG)/actualAngle;
+}
+
+void CYS8218Controller::Set(float angle)
+{
+  _angle = angle;
+  SetServo();
+}
+
+void CYS8218Controller::Move(float relAngle)
+{
+  _angle += relAngle;
+  SetServo();
+}