This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Revision 63:c2c6269767b8, committed 2013-04-14
- Comitter:
- madcowswe
- Date:
- Sun Apr 14 14:32:43 2013 +0000
- Parent:
- 62:78d99b781f02
- Child:
- 64:c979fb1cd3b5
- Commit message:
- Workinf feed forward both turn and fwd
Changed in this revision
Processes/MotorControl/MotorControl.cpp | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/Processes/MotorControl/MotorControl.cpp Sun Apr 14 12:57:04 2013 +0000 +++ b/Processes/MotorControl/MotorControl.cpp Sun Apr 14 14:32:43 2013 +0000 @@ -25,12 +25,12 @@ float testspeed = 0.2; float Fcrit = 1.75; - float Pcrit = 10*0.5; + float Pcrit = 10; float Pgain = Pcrit*0.45; - float Igain = 1.2f*Pgain*Fcrit*0.2; + float Igain = 1.2f*Pgain*Fcrit*0.05; float testrot = 0.5*PI; - float Pcrit_rot = 10; + float Pcrit_rot = 20; float Pgain_rot = Pcrit_rot*0.45f; float Fcrit_rot = 1.75f; float Igain_rot = 1.2f*Pgain_rot*Fcrit_rot*0.1; @@ -53,7 +53,7 @@ float currtime = SystemTime.read(); float dt = currtime - lastT; - dt = 0.05; //TODO: HACK! + //dt = 0.05; //TODO: HACK! lastT = currtime; thetafiltstate = MOTORCONTROLLER_FILTER_K * thetafiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright-dleft)/(dt*ENCODER_WHEELBASE)); @@ -64,20 +64,26 @@ static float fwdIstate = 0; static float rotIstate = 0; - fwdIstate += errfwd; - rotIstate += errtheta; - float actuatefwd = errfwd*Pgain + fwdIstate*Igain + max(power_per_dc_m_per_s*abs(fwdcmd), hysteresis_pwr)*sgn(fwdcmd); + float actuatefwd = errfwd*Pgain + fwdIstate*Igain;// + max(power_per_dc_m_per_s*abs(fwdcmd), hysteresis_pwr)*sgn(fwdcmd); float actuaterot = errtheta*Pgain_rot + rotIstate*Igain_rot; float actuateleft = actuatefwd - (actuaterot*ENCODER_WHEELBASE/2.0f); float actuateright = actuatefwd + (actuaterot*ENCODER_WHEELBASE/2.0f); + + float lff = fwdcmd - omegacmd*ENCODER_WHEELBASE/2.0f; + float rff = fwdcmd + omegacmd*ENCODER_WHEELBASE/2.0f; - mleft(max(min(actuateleft, MOTOR_MAX_POWER), -MOTOR_MAX_POWER)); - mright(max(min(actuateright, MOTOR_MAX_POWER), -MOTOR_MAX_POWER)); + mleft(max(min(actuateleft + max(power_per_dc_m_per_s*abs(lff), hysteresis_pwr)*sgn(lff), MOTOR_MAX_POWER), -MOTOR_MAX_POWER)); + mright(max(min(actuateright + max(power_per_dc_m_per_s*abs(rff), hysteresis_pwr)*sgn(rff), MOTOR_MAX_POWER), -MOTOR_MAX_POWER)); - mfwdpowdbg = fwdIstate*Igain; - mrotpowdbg = rotIstate*Igain_rot; + if (!(abs(actuateleft) > MOTOR_MAX_POWER || abs(actuateright) > MOTOR_MAX_POWER)){ + fwdIstate += errfwd; + rotIstate += errtheta; + } + + //mfwdpowdbg = 0;//fwdfiltstate;//; + //mrotpowdbg = rotIstate*Igain_rot;//thetafiltstate;//; }
--- a/main.cpp Sun Apr 14 12:57:04 2013 +0000 +++ b/main.cpp Sun Apr 14 14:32:43 2013 +0000 @@ -101,6 +101,20 @@ Thread::wait(5000); } + MotorControl::set_fwdcmd(0); + for(float spd = 0.05; spd <= 2; spd *= 1.4){ + + MotorControl::set_omegacmd(spd); + + Thread::wait(3000); + + float f = MotorControl::mfwdpowdbg; + float r = MotorControl::mrotpowdbg; + MotorControl::set_omegacmd(0); + printf("targetspd:%f, fwd:%f, rot:%f\r\n", spd, f, r); + Thread::wait(5000); + } + Thread::wait(osWaitForever); }