MotorDriver.cpp

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