Diff: ServoDriver_new/ServoDriver.cpp

Revision:

74:d281aaef9766

Parent:

73:f6f378311c8d

--- a/ServoDriver_new/ServoDriver.cpp	Mon Jul 30 16:48:48 2018 +0000
+++ b/ServoDriver_new/ServoDriver.cpp	Tue Aug 14 21:06:48 2018 +0000
@@ -16,13 +16,15 @@
 // note: (min_deg < max_deg) must be true!
 ServoDriver::ServoDriver(PinName digital_pin) : _pwm_out(digital_pin) {
     _period_cnt = 0;
-    _center_pwm = 1500.0; // mechanical center trimmed to be 1.5ms (1500us)
+    _center_pwm = 1640.0; // mechanical center trimmed to be 1.5ms (1500us)
     _min_deg = -45.0;     // lower deflection against end stop
     _max_deg =  45.0;     // upper deflection against end stop
     _min_pwm = 2240.0;    // pwm corresponding to _min_deg
     _max_pwm = 1040.0;    // pwm corresponding to _max_deg
     
     _valid_servo_position_pwm = (unsigned int)_center_pwm; // servo position on boot-up
+    
+    _paused = false;
 }
 
 /////////////////////////////////////////////////////////////////////////////
@@ -41,16 +43,21 @@
 }
 
 void ServoDriver::pwm_pulse_on() {
-    pwm_pulse_off_timeout.attach_us(callback(this, &ServoDriver::pwm_pulse_off), _valid_servo_position_pwm);  // _valid_servo_position_pwm is the signal in microseconds, e.g. 1500
+    if (_paused) {
+        pwm_pulse_off_timeout.detach();
+    }
+    else {
+        pwm_pulse_off_timeout.attach_us(callback(this, &ServoDriver::pwm_pulse_off), _valid_servo_position_pwm);  // _valid_servo_position_pwm is the signal in microseconds, e.g. 1500
+    }
     _pwm_out = 1;
 }
 
 void ServoDriver::pause() {
-    pwm_pulse_on_ticker.detach();     //run a pwm pulse on a digital out pin that has a period of 20 ms
+    //pwm_pulse_on_ticker.detach();     //run a pwm pulse on a digital out pin that has a period of 20 ms
 }
 
 void ServoDriver::unpause() {
-    init();
+    //init();
 }
 
 // SERVO OUTPUT MAPPING
@@ -60,8 +67,6 @@
     // make sure desired angle is within the min and max deflection angles
     _degrees_set_position = servoClamp<float>(input_deg, _min_deg, _max_deg);
 
-#if(0)
-
     // servo calibration
     // slope is pwm signal over degrees (y = mx + b) using linear fit
     // different slopes for above or below center!
@@ -81,18 +86,22 @@
 
     // saturate the pwm output to the min/max pwm limits
     // remember the min/max pwm values may not be (min < max)
+    // Basically handles the direction of movement
     if (_max_pwm > _min_pwm)
         _valid_servo_position_pwm = servoClamp<float>(initial_output, _min_pwm, _max_pwm);
         
     else if (_max_pwm < _min_pwm)
         _valid_servo_position_pwm = servoClamp<float>(initial_output, _max_pwm, _min_pwm);
-#else
-    float slope, out;
+}
+
+//the comparison below is used to swap the rotation of the servo
+
+void ServoDriver::setPWM(float input_pwm) {
+    if (_max_pwm > _min_pwm)
+        _valid_servo_position_pwm = (unsigned int) servoClamp<float>(input_pwm, _min_pwm, _max_pwm);   //volatile unsigned int
     
-    slope = (_max_pwm - _min_pwm)/( _max_deg - _min_deg );
-    out = slope*(_degrees_set_position - _min_deg) + _min_pwm;
-    _valid_servo_position_pwm = (unsigned int)out;
-#endif
+    else if (_min_pwm > _max_pwm)
+        _valid_servo_position_pwm = (unsigned int) servoClamp<float>(input_pwm, _max_pwm, _min_pwm);   //volatile unsigned int
 }
 
 ///////////////////////////////////////////////////////////////////////////
@@ -119,12 +128,12 @@
 
 ///////////////////////////////////////////////////////////////////////////
 // READING PARAMETERS
-float ServoDriver::getPosition_deg() {    //rename to getSetPosition later for consistency
+float ServoDriver::getSetPosition_deg() {    //rename to getSetPosition later for consistency
     return _degrees_set_position;
 }
     
 // returns pwm signal in microseconds, for example: 1580 microseconds
-float ServoDriver::getPosition_pwm() {
+float ServoDriver::getSetPosition_pwm() {
     return (float)_valid_servo_position_pwm;    //servo is unsigned int
 }