File content as of revision 92:52a91656458a:

// modifications by Dan 2018-02-28
// - mostly formatting consistency for readability
// - changed ServoDriver initial _valid_servo_position to the _center_pwm value
// - fixed problems with servo mapping function.  
// -- made min_pwm match to min_deg, even if min_pwm is greater than max_pwm
// -- made max_pwm match to max_deg, even if max_pwm is less than min_pwm
// -- now linear mapping between min pwm/deg, center_pwm/0deg, max pwm/deg.     

#include "ServoDriver.hpp"

// INNER LOOP ServoDriver has settings, endpoints, direction, neutral point

//this driver does not use pwmOut because of the period timer conflict with the H-bridge motor controllers

// initialize default calibration/parameter values
// note: (min_deg < max_deg) must be true!
ServoDriver::ServoDriver(PinName digital_pin) : _pwm_out(digital_pin) {
    _period_cnt = 0;
    _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;
}

/////////////////////////////////////////////////////////////////////////////
// PWM TIMING SETUP AND SUPPORT FUNCTIONS

// initialize the ticker that will pulse on, start an off ticker, off-ticker pulses off and detaches, rinse and repeat
void ServoDriver::init() {
}

void ServoDriver::runServo() {
    pwm_pulse_on();
}

void ServoDriver::pwm_pulse_off() {
    _pwm_out = 0;                       //changed the pulse on and off to TIMEOUT so you don't have to detach anything
}

void ServoDriver::pwm_pulse_on() {
    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
}

void ServoDriver::unpause() {
    //init();
}

// SERVO OUTPUT MAPPING

// this function converts a desired degree location into a PWM command
void ServoDriver::setPosition_deg(float input_deg) {
    // make sure desired angle is within the min and max deflection angles
    _degrees_set_position = servoClamp<float>(input_deg, _min_deg, _max_deg);

    // servo calibration
    // slope is pwm signal over degrees (y = mx + b) using linear fit
    // different slopes for above or below center!
    float slope;
    if (_degrees_set_position > 0)
        slope = (_center_pwm - _min_pwm) / (-_min_deg);

    else if (_degrees_set_position < 0)
        slope = (_max_pwm - _center_pwm) / (_max_deg);

    else
        slope = 0.0;

    // calculate linear servo response (output = slope*input + offset)
    // negative slope needed for correct direction response
    float initial_output = (slope * _degrees_set_position) + _center_pwm;    

    // 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);
}

//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
    
    else if (_min_pwm > _max_pwm)
        _valid_servo_position_pwm = (unsigned int) servoClamp<float>(input_pwm, _max_pwm, _min_pwm);   //volatile unsigned int
}

///////////////////////////////////////////////////////////////////////////
// SETTING PARAMETERS
void ServoDriver::setMinPWM(float pwm_input) {
    _min_pwm = pwm_input;
}

void ServoDriver::setMaxPWM(float pwm_input) {
    _max_pwm = pwm_input;
}

void ServoDriver::setCenterPWM(float pwm_input) {
    _center_pwm = pwm_input;
}

void ServoDriver::setMinDeg(float deg) {
    _min_deg = deg;
}

void ServoDriver::setMaxDeg(float deg) {
    _max_deg = deg;
}

///////////////////////////////////////////////////////////////////////////
// READING PARAMETERS
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::getSetPosition_pwm() {
    return (float)_valid_servo_position_pwm;    //servo is unsigned int
}

float ServoDriver::getMinPWM() {
    return _min_pwm;
}

float ServoDriver::getMaxPWM() {
    return _max_pwm;
}

float ServoDriver::getCenterPWM() {
    return _center_pwm;
}

float ServoDriver::getMinDeg() {
    return _min_deg;
}

float ServoDriver::getMaxDeg() {
    return _max_deg;
}