This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Diff: Processes/MotorControl/MotorControl.cpp
- Revision:
- 22:6e3218cf75f8
- Child:
- 25:b16f1045108f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Processes/MotorControl/MotorControl.cpp Tue Apr 09 20:41:22 2013 +0000 @@ -0,0 +1,51 @@ + +#include "MainMotor.h" +#include "Encoder.h" +#include "globals.h" +#include <algorithm> + +namespace MotorControl{ + +float fwdcmd = 0; +float thetacmd = 0; + +void motor_control_loop(){ + MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B); + + float Pgain = -0.01; + static float lastT = SystemTime; + static float lastright = right_encoder.getTicks() * ENCODER_M_PER_TICK; + static float lastleft = left_encoder.getTicks() * ENCODER_M_PER_TICK; + + while(true){ + + static float thetafiltstate = 0; + static float fwdfiltstate = 0; + + float currright = right_encoder.getTicks() * ENCODER_M_PER_TICK; + float dright = currright - lastright; + lastright = currright; + + float currleft = left_encoder.getTicks() * ENCODER_M_PER_TICK; + float dleft = currleft - lastleft; + currleft = currleft; + + float currtime = SystemTime.read(); + float dt = currtime - lastT; + lastT = currtime; + + 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 - thetacmd; + + float errleft = errfwd - (errtheta*ENCODER_WHEELBASE/2.0f); + float errright = errfwd + (errtheta*ENCODER_WHEELBASE/2.0f); + + mleft(max(min(errleft*Pgain, MOTOR_MAX_POWER), -MOTOR_MAX_POWER)); + mright(max(min(errright*Pgain, MOTOR_MAX_POWER), -MOTOR_MAX_POWER)); + } +} + +} \ No newline at end of file