File content as of revision 3:7a6365365ee2:

#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.0f/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 = MOTOR_ERROR;       // 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.0f) && (motor_state.code == DRIVING_CW) && (_pwm > 0.05f)) ||
        (bool)((speed > 0.0f) && (motor_state.code == DRIVING_CCW)&& (_pwm > 0.05f));
    if(isTryingToInstantReverse) {
    // Set motor to brake, set state to error
        coast();
        motor_state.code = MOTOR_ERROR;
        motor_state.value = _pwm;
        return motor_state;
    } else {
        if(speed == 0.0f) {
        // No effect to _in pins
        } else {
            _in1 = (speed>0.0f);
            _in2 = (speed<0.0f);
        }
        _pwm = std::abs(speed);
        determineState();
        return motor_state;
    }
}

State_t MotorDriver::forceSetSpeed(float speed)
{
    if(speed == 0.0f) {
        // No effect to _in pins
    } else {
        _in1 = (speed>0.0f);
        _in2 = (speed<0.0f);
    }
    _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.0f - 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;
}