File content as of revision 3:9e51de1050a1:

#include "MotorDriver.h"
#include "mbed.h"
#include <cmath>

// See .h file for function descriptions and details

MotorDriver::MotorDriver(DigitalOut in1, DigitalOut in2, PwmOut pwm, float pwmFreq, bool isBrakeable) 
                        : _in1(in1), _in2(in2), _pwm(pwm) 
{
    // Initialize motor and variables
    _in1 = 1; _in2 = 0; _pwm = 0.0; this->isBrakeable = isBrakeable;
    _pwm.period(1.0/pwmFreq);
    determineState();
}

void MotorDriver::determineState(){
    if(_in1==1 && _in2==1){
        motor_state.code  = BRAKING;
        motor_state.value = _pwm;    
    }
    else if(_in1==1 && _in2==0){
        motor_state.code  = DRIVING_CW;
        motor_state.value = _pwm;
    }
    else if(_in1==0 && _in2==1){
        motor_state.code  = DRIVING_CCW;
        motor_state.value = _pwm;  
    }
    else if(_in1==0 && _in2==0){
        motor_state.code = COASTING;
        motor_state.value = _pwm;
    }
    else{
        motor_state.code = ERROR_M;       // Undefined config found
        motor_state.value = _pwm;
    }    
}

State_t MotorDriver::setSpeed(float speed){
    // Prevent instantaneous reversal; put into brake if requested to do so
    bool isTryingToInstantReverse = 
         (bool)((speed < 0.0) && (motor_state.code == DRIVING_CW) && (_pwm > 0.05)) || 
         (bool)((speed > 0.0) && (motor_state.code == DRIVING_CCW)&& (_pwm > 0.05));
    if(isTryingToInstantReverse){
        // Set motor to brake, set state to error
        coast();
        motor_state.code = ERROR_M; motor_state.value = _pwm;
        return motor_state;
    }
    else{
        if(speed == 0.0)
        {
            // No effect to _in pins
        }
        else{
            _in1 = (speed>0.0);
            _in2 = (speed<0.0);
        }
        _pwm = std::abs(speed);
        determineState();
        return motor_state;
    }
}

State_t MotorDriver::forceSetSpeed(float speed){
    if(speed == 0.0)
        {
            // No effect to _in pins
        }
        else{
            _in1 = (speed>0.0);
            _in2 = (speed<0.0);
        }
        _pwm = std::abs(speed);
        determineState();
        return motor_state;
}

State_t MotorDriver::brake(float intensity){
    if(!isBrakeable) coast();
    else{
        _in1=1; _in2=1; _pwm = _pwm * (1.0-intensity);  
    }
    determineState();
    return motor_state;
}

State_t MotorDriver::coast(){
    _in1=0; _in2=0; _pwm = 1.0;
    determineState();
    return motor_state;
}

State_t MotorDriver::getState(){
    return motor_state;
}