test
Fork of MotorDriver by
MotorDriver.cpp
- Committer:
- electromotivated
- Date:
- 2015-12-08
- Revision:
- 0:871adb9cf798
- Child:
- 1:19476b49822b
File content as of revision 0:871adb9cf798:
#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; // 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; 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; }