2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/MotorControl/MotorControl.cpp
- Revision:
- 60:5058465904e0
- Parent:
- 34:e1678450feec
- Child:
- 62:78d99b781f02
--- a/Processes/MotorControl/MotorControl.cpp Fri Apr 12 22:00:32 2013 +0000 +++ b/Processes/MotorControl/MotorControl.cpp Sat Apr 13 22:41:00 2013 +0000 @@ -8,13 +8,15 @@ namespace MotorControl { -float fwdcmd = 0; -float omegacmd = 0; +volatile float fwdcmd = 0; +volatile float omegacmd = 0; void motor_control_isr() { MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B); + + const float power_per_dc_m_per_s = 1.0f/0.6f; float testspeed = 0.2; float Fcrit = 1.75; @@ -51,15 +53,15 @@ thetafiltstate = MOTORCONTROLLER_FILTER_K * thetafiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright-dleft)/(dt*ENCODER_WHEELBASE)); fwdfiltstate = MOTORCONTROLLER_FILTER_K * fwdfiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright+dleft)/(2.0f*dt)); - float errfwd = fwdfiltstate - fwdcmd; - float errtheta = thetafiltstate - omegacmd; + float errfwd = fwdcmd - fwdfiltstate; + float errtheta = omegacmd - thetafiltstate; static float fwdIstate = 0; static float rotIstate = 0; fwdIstate += errfwd; rotIstate += errtheta; - float actuatefwd = errfwd*Pgain + fwdIstate*Igain; + float actuatefwd = errfwd*Pgain + fwdIstate*Igain + power_per_dc_m_per_s*fwdcmd; float actuaterot = errtheta*Pgain_rot + rotIstate*Igain_rot; float actuateleft = actuatefwd - (actuaterot*ENCODER_WHEELBASE/2.0f);