Diff: Processes/MotorControl/MotorControl.cpp

Revision:

73:265d3cc6b0b1

Parent:

64:c979fb1cd3b5

Child:

75:283283604485

--- a/Processes/MotorControl/MotorControl.cpp	Mon Apr 15 13:37:32 2013 +0000
+++ b/Processes/MotorControl/MotorControl.cpp	Mon Apr 15 13:44:49 2013 +0000
@@ -9,6 +9,8 @@
 namespace MotorControl
 {
 
+volatile bool motorsenabled = 0;
+
 volatile float fwdcmd = 0;
 volatile float omegacmd = 0;
 
@@ -19,7 +21,6 @@
 
 void motor_control_isr()
 {
-    
     const float power_per_dc_m_per_s = 1.64f;
     const float hysteresis_pwr = 0.16f;
 
@@ -73,13 +74,20 @@
     
     float lff = fwdcmd - omegacmd*ENCODER_WHEELBASE/2.0f;
     float rff = fwdcmd + omegacmd*ENCODER_WHEELBASE/2.0f;
-
-    mleft(max(min(actuateleft + max(power_per_dc_m_per_s*abs(lff), min(hysteresis_pwr, 3.0f*abs(lff)))*sgn(lff), MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
-    mright(max(min(actuateright + max(power_per_dc_m_per_s*abs(rff), min(hysteresis_pwr, 3.0f*abs(rff)))*sgn(rff), MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
     
-    if (!(abs(actuateleft) > MOTOR_MAX_POWER || abs(actuateright) > MOTOR_MAX_POWER)){
-        fwdIstate += errfwd;
-        rotIstate += errtheta;
+    if(motorsenabled){
+
+        mleft(max(min(actuateleft + max(power_per_dc_m_per_s*abs(lff), min(hysteresis_pwr, 3.0f*abs(lff)))*sgn(lff), MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
+        mright(max(min(actuateright + max(power_per_dc_m_per_s*abs(rff), min(hysteresis_pwr, 3.0f*abs(rff)))*sgn(rff), MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
+        
+        if (!(abs(actuateleft) > MOTOR_MAX_POWER || abs(actuateright) > MOTOR_MAX_POWER)){
+            fwdIstate += errfwd;
+            rotIstate += errtheta;
+        }
+    
+    } else {
+        fwdIstate = 0;
+        rotIstate = 0;
     }
     
     //mfwdpowdbg = 0;//fwdfiltstate;//;