2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/MotorControl/MotorControl.cpp
- Revision:
- 73:265d3cc6b0b1
- Parent:
- 64:c979fb1cd3b5
- Child:
- 75:283283604485
diff -r 7996aa8286ae -r 265d3cc6b0b1 Processes/MotorControl/MotorControl.cpp --- 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;//;