Diff: RC_Servo.cpp

Revision:

6:cf65fc8b0de1

Parent:

1:8482eba4d652

Child:

7:014d36c33b73

--- a/RC_Servo.cpp	Thu May 31 17:26:54 2018 +0000
+++ b/RC_Servo.cpp	Tue Jun 05 07:25:09 2018 +0000
@@ -1,21 +1,40 @@
 #include "RC_Servo.h"
 
-RC_Servo::RC_Servo(PinName PWM, int _extended) : _pwm(PWM)
+RC_Servo::RC_Servo(PinName PWM, int _extended) : _RCpwm(PWM)
 {
-    _pwm.period(0.02);
+    _tickRC.attach(callback(this,&RC_Servo::generatePwm),0.02);
     if (_extended) {
-        _pMin = 400;
-        _pMax = 2400;
+        _RCpMin = 400;
+        _RCpMax = 2400;
     } else {
-        _pMin = 1000;
-        _pMax = 2000;
+        _RCpMin = 1000;
+        _RCpMax = 2000;
     }
 }
 
-int RC_Servo::setLimits (int Tmin, int Tmax) {
-    if ((Tmin > 400) && (Tmin < Tmax))  _pMin = Tmin;
+void RC_Servo::generatePwm (void)
+{
+
+    static int  output = 0;
+    static long pulseWidthTime = 0;
+
+    if (pulseWidthTime == 0) pulseWidthTime = _RCpMin;
+    _tickRC.detach();
+    output = !output;
+    _RCpwm = output;
+
+    if (output == 1)    _tickRC.attach (callback(this,&RC_Servo::generatePwm), pulseWidthTime);
+    else {
+        _tickRC.attach (callback(this,&RC_Servo::generatePwm), 20000-pulseWidthTime);
+        pulseWidthTime = RCdelay;
+    }
+}
+
+int RC_Servo::setLimits (int Tmin, int Tmax)
+{
+    if ((Tmin > 400) && (Tmin < Tmax))  _RCpMin = Tmin;
     else return Tmin;
-    if ((Tmax < 2400) && (Tmin < Tmax)) _pMax = Tmax;
+    if ((Tmax < 2400) && (Tmin < Tmax)) _RCpMax = Tmax;
     else return Tmax;
     return 0;
 }
@@ -23,7 +42,7 @@
 void RC_Servo::write (float position)
 {
     if ((position >= 0) && (position <= 1))
-        _pwm.pulsewidth_us (_pMin + (int)(position * (_pMax - _pMin)));
+        RCdelay = _RCpMin + (long)((_RCpMax - _RCpMin) * position);
 }
 
 RC_Servo& RC_Servo::operator= (float position)