2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Processes/MotorControl/MotorControl.cpp
- Committer:
- madcowswe
- Date:
- 2013-04-09
- Revision:
- 22:6e3218cf75f8
- Child:
- 25:b16f1045108f
File content as of revision 22:6e3218cf75f8:
#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)); } } }