ServoDriver.cpp

00001 // modifications by Dan 2018-02-28
00002 // - mostly formatting consistency for readability
00003 // - changed ServoDriver initial _valid_servo_position to the _center_pwm value
00004 // - fixed problems with servo mapping function.  
00005 // -- made min_pwm match to min_deg, even if min_pwm is greater than max_pwm
00006 // -- made max_pwm match to max_deg, even if max_pwm is less than min_pwm
00007 // -- now linear mapping between min pwm/deg, center_pwm/0deg, max pwm/deg.     
00008 
00009 #include "ServoDriver.hpp"
00010 
00011 // INNER LOOP ServoDriver has settings, endpoints, direction, neutral point
00012 
00013 //this driver does not use pwmOut because of the period timer conflict with the H-bridge motor controllers
00014 
00015 // initialize default calibration/parameter values
00016 // note: (min_deg < max_deg) must be true!
00017 ServoDriver::ServoDriver(PinName digital_pin) : _pwm_out(digital_pin) {
00018     _period_cnt = 0;
00019     _center_pwm = 1640.0; // mechanical center trimmed to be 1.5ms (1500us)
00020     _min_deg = -45.0;     // lower deflection against end stop
00021     _max_deg =  45.0;     // upper deflection against end stop
00022     _min_pwm = 2240.0;    // pwm corresponding to _min_deg
00023     _max_pwm = 1040.0;    // pwm corresponding to _max_deg
00024     
00025     _valid_servo_position_pwm = (unsigned int)_center_pwm; // servo position on boot-up
00026     
00027     _paused = false;
00028 }
00029 
00030 /////////////////////////////////////////////////////////////////////////////
00031 // PWM TIMING SETUP AND SUPPORT FUNCTIONS
00032 
00033 // initialize the ticker that will pulse on, start an off ticker, off-ticker pulses off and detaches, rinse and repeat
00034 void ServoDriver::init() {
00035 }
00036 
00037 void ServoDriver::runServo() {
00038     pwm_pulse_on();
00039 }
00040 
00041 void ServoDriver::pwm_pulse_off() {
00042     _pwm_out = 0;                       //changed the pulse on and off to TIMEOUT so you don't have to detach anything
00043 }
00044 
00045 void ServoDriver::pwm_pulse_on() {
00046     if (_paused) {
00047         pwm_pulse_off_timeout.detach();
00048     }
00049     else {
00050         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
00051     }
00052     _pwm_out = 1;
00053 }
00054 
00055 void ServoDriver::pause() {
00056     //pwm_pulse_on_ticker.detach();     //run a pwm pulse on a digital out pin that has a period of 20 ms
00057 }
00058 
00059 void ServoDriver::unpause() {
00060     //init();
00061 }
00062 
00063 // SERVO OUTPUT MAPPING
00064 
00065 // this function converts a desired degree location into a PWM command
00066 void ServoDriver::setPosition_deg(float input_deg) {
00067     // make sure desired angle is within the min and max deflection angles
00068     _degrees_set_position = servoClamp<float>(input_deg, _min_deg, _max_deg);
00069 
00070     // servo calibration
00071     // slope is pwm signal over degrees (y = mx + b) using linear fit
00072     // different slopes for above or below center!
00073     float slope;
00074     if (_degrees_set_position > 0)
00075         slope = (_center_pwm - _min_pwm) / (-_min_deg);
00076 
00077     else if (_degrees_set_position < 0)
00078         slope = (_max_pwm - _center_pwm) / (_max_deg);
00079 
00080     else
00081         slope = 0.0;
00082 
00083     // calculate linear servo response (output = slope*input + offset)
00084     // negative slope needed for correct direction response
00085     float initial_output = (slope * _degrees_set_position) + _center_pwm;    
00086 
00087     // saturate the pwm output to the min/max pwm limits
00088     // remember the min/max pwm values may not be (min < max)
00089     // Basically handles the direction of movement
00090     if (_max_pwm > _min_pwm)
00091         _valid_servo_position_pwm = servoClamp<float>(initial_output, _min_pwm, _max_pwm);
00092         
00093     else if (_max_pwm < _min_pwm)
00094         _valid_servo_position_pwm = servoClamp<float>(initial_output, _max_pwm, _min_pwm);
00095 }
00096 
00097 //the comparison below is used to swap the rotation of the servo
00098 
00099 void ServoDriver::setPWM(float input_pwm) {
00100     if (_max_pwm > _min_pwm)
00101         _valid_servo_position_pwm = (unsigned int) servoClamp<float>(input_pwm, _min_pwm, _max_pwm);   //volatile unsigned int
00102     
00103     else if (_min_pwm > _max_pwm)
00104         _valid_servo_position_pwm = (unsigned int) servoClamp<float>(input_pwm, _max_pwm, _min_pwm);   //volatile unsigned int
00105 }
00106 
00107 ///////////////////////////////////////////////////////////////////////////
00108 // SETTING PARAMETERS
00109 void ServoDriver::setMinPWM(float pwm_input) {
00110     _min_pwm = pwm_input;
00111 }
00112 
00113 void ServoDriver::setMaxPWM(float pwm_input) {
00114     _max_pwm = pwm_input;
00115 }
00116 
00117 void ServoDriver::setCenterPWM(float pwm_input) {
00118     _center_pwm = pwm_input;
00119 }
00120 
00121 void ServoDriver::setMinDeg(float deg) {
00122     _min_deg = deg;
00123 }
00124 
00125 void ServoDriver::setMaxDeg(float deg) {
00126     _max_deg = deg;
00127 }
00128 
00129 ///////////////////////////////////////////////////////////////////////////
00130 // READING PARAMETERS
00131 float ServoDriver::getSetPosition_deg() {    //rename to getSetPosition later for consistency
00132     return _degrees_set_position;
00133 }
00134     
00135 // returns pwm signal in microseconds, for example: 1580 microseconds
00136 float ServoDriver::getSetPosition_pwm() {
00137     return (float)_valid_servo_position_pwm;    //servo is unsigned int
00138 }
00139 
00140 float ServoDriver::getMinPWM() {
00141     return _min_pwm;
00142 }
00143 
00144 float ServoDriver::getMaxPWM() {
00145     return _max_pwm;
00146 }
00147 
00148 float ServoDriver::getCenterPWM() {
00149     return _center_pwm;
00150 }
00151 
00152 float ServoDriver::getMinDeg() {
00153     return _min_deg;
00154 }
00155 
00156 float ServoDriver::getMaxDeg() {
00157     return _max_deg;
00158 }