2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/MotorControl/MotorControl.cpp
- Revision:
- 33:a49197572737
- Parent:
- 28:4e20b44251c6
- Child:
- 34:e1678450feec
diff -r e3f633620816 -r a49197572737 Processes/MotorControl/MotorControl.cpp --- a/Processes/MotorControl/MotorControl.cpp Wed Apr 10 18:25:16 2013 +0000 +++ b/Processes/MotorControl/MotorControl.cpp Wed Apr 10 19:52:19 2013 +0000 @@ -17,7 +17,18 @@ MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B); float testspeed = 0.2; - float Pgain = 10; + float Fcrit = 1.75; + float Pcrit = 10; + float Pgain = Pcrit*0.45; + float Igain = 1.2f*Pgain*Fcrit*0.2; + + float testrot = 0.5*PI; + float Pcrit_rot = 10; + float Pgain_rot = Pcrit_rot*0.45f; + float Fcrit_rot = 1.75f; + float Igain_rot = 1.2f*Pgain_rot*Fcrit_rot*0.07; + + //float Dgain = static float lastT = SystemTime.read(); static float lastright = right_encoder.getTicks() * ENCODER_M_PER_TICK; static float lastleft = left_encoder.getTicks() * ENCODER_M_PER_TICK; @@ -42,12 +53,20 @@ float errfwd = fwdfiltstate - fwdcmd; float errtheta = thetafiltstate - omegacmd; + + static float fwdIstate = 0; + static float rotIstate = 0; + fwdIstate += errfwd; + rotIstate += errtheta; + + float actuatefwd = errfwd*Pgain + fwdIstate*Igain; + float actuaterot = errtheta*Pgain_rot + rotIstate*Igain_rot; - float errleft = errfwd - (errtheta*ENCODER_WHEELBASE/2.0f); - float errright = errfwd + (errtheta*ENCODER_WHEELBASE/2.0f); + float actuateleft = actuatefwd - (actuaterot*ENCODER_WHEELBASE/2.0f); + float actuateright = actuatefwd + (actuaterot*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)); + mleft(max(min(actuateleft, MOTOR_MAX_POWER), -MOTOR_MAX_POWER)); + mright(max(min(actuateright, MOTOR_MAX_POWER), -MOTOR_MAX_POWER)); }