Diff: MotorDriver.cpp

Revision:

0:871adb9cf798

Child:

1:19476b49822b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorDriver.cpp	Tue Dec 08 00:11:04 2015 +0000
@@ -0,0 +1,96 @@
+#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;
+}
\ No newline at end of file