MotorDriver.cpp

00001 #include "MotorDriver.h"
00002 #include "mbed.h"
00003 #include <cmath>
00004 
00005 // See .h file for function descriptions and details
00006 MotorDriver::MotorDriver(DigitalOut in1, DigitalOut in2, PwmOut pwm, float pwmFreq, bool isBrakeable)
00007     : _in1(in1), _in2(in2), _pwm(pwm)
00008 {
00009     // Initialize motor and variables
00010     _in1 = 1;
00011     _in2 = 0;
00012     _pwm = 0.0;
00013     this->isBrakeable = isBrakeable;
00014     _pwm.period(1.0f/pwmFreq);
00015     determineState();
00016 }
00017 
00018 void MotorDriver::determineState()
00019 {
00020     if(_in1==1 && _in2==1) {
00021         motor_state.code  = BRAKING;
00022         motor_state.value = _pwm;
00023     } else if(_in1==1 && _in2==0) {
00024         motor_state.code  = DRIVING_CW;
00025         motor_state.value = _pwm;
00026     } else if(_in1==0 && _in2==1) {
00027         motor_state.code  = DRIVING_CCW;
00028         motor_state.value = _pwm;
00029     } else if(_in1==0 && _in2==0) {
00030         motor_state.code = COASTING;
00031         motor_state.value = _pwm;
00032     } else {
00033         motor_state.code = MOTOR_ERROR;       // Undefined config found
00034         motor_state.value = _pwm;
00035     }
00036 }
00037 
00038 State_t MotorDriver::setSpeed(float speed)
00039 {
00040     // Prevent instantaneous reversal; put into brake if requested to do so
00041     bool isTryingToInstantReverse =
00042         (bool)((speed < 0.0f) && (motor_state.code == DRIVING_CW) && (_pwm > 0.05f)) ||
00043         (bool)((speed > 0.0f) && (motor_state.code == DRIVING_CCW)&& (_pwm > 0.05f));
00044     if(isTryingToInstantReverse) {
00045     // Set motor to brake, set state to error
00046         coast();
00047         motor_state.code = MOTOR_ERROR;
00048         motor_state.value = _pwm;
00049         return motor_state;
00050     } else {
00051         if(speed == 0.0f) {
00052         // No effect to _in pins
00053         } else {
00054             _in1 = (speed>0.0f);
00055             _in2 = (speed<0.0f);
00056         }
00057         _pwm = std::abs(speed);
00058         determineState();
00059         return motor_state;
00060     }
00061 }
00062 
00063 State_t MotorDriver::forceSetSpeed(float speed)
00064 {
00065     if(speed == 0.0f) {
00066         // No effect to _in pins
00067     } else {
00068         _in1 = (speed>0.0f);
00069         _in2 = (speed<0.0f);
00070     }
00071     _pwm = std::abs(speed);
00072     determineState();
00073     return motor_state;
00074 }
00075 
00076 State_t MotorDriver::brake(float intensity)
00077 {
00078     if(!isBrakeable) coast();
00079     else {
00080         _in1=1;
00081         _in2=1;
00082         _pwm = _pwm * (1.0f - intensity);
00083     }
00084     determineState();
00085     return motor_state;
00086 }
00087 
00088 State_t MotorDriver::coast()
00089 {
00090     _in1=0;
00091     _in2=0;
00092     _pwm = 1.0;
00093     determineState();
00094     return motor_state;
00095 }
00096 
00097 State_t MotorDriver::getState()
00098 {
00099     return motor_state;
00100 }