![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Processes/MotorControl/MotorControl.cpp
- Committer:
- madcowswe
- Date:
- 2013-04-17
- Revision:
- 90:e4164bb8c60e
- Parent:
- 88:8850373c3f0d
File content as of revision 90:e4164bb8c60e:
#include "MainMotor.h" #include "Encoder.h" #include "globals.h" #include <algorithm> #include "system.h" #include "supportfuncs.h" namespace MotorControl { volatile bool motorsenabled = 0; volatile float fwdcmd = 0; volatile float omegacmd = 0; volatile float mfwdpowdbg = 0; volatile float mrotpowdbg = 0; MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B); void motor_control_isr() { const float power_per_dc_m_per_s = 1.64f; const float hysteresis_pwr = 0.16f; float testspeed = 0.2; float Fcrit = 1.75; float Pcrit = 8; float Pgain = Pcrit*0.45; float Igain = 1.2f*Pgain*Fcrit*0.05; float testrot = 0.5*PI; 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; //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; 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; lastleft = currleft; float currtime = SystemTime.read(); float dt = currtime - lastT; //dt = 0.05; //TODO: HACK! 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 = fwdcmd - fwdfiltstate; float errtheta = omegacmd - thetafiltstate; static float fwdIstate = 0; static float rotIstate = 0; 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; if(motorsenabled){ mleft(max(min(actuateleft + max(power_per_dc_m_per_s*abs(lff), min(hysteresis_pwr, 3.0f*abs(lff)))*sgn(lff), MOTOR_MAX_POWER), -MOTOR_MAX_POWER)); mright(max(min(actuateright + max(power_per_dc_m_per_s*abs(rff), min(hysteresis_pwr, 3.0f*abs(rff)))*sgn(rff), MOTOR_MAX_POWER), -MOTOR_MAX_POWER)); if (!(abs(actuateleft) > MOTOR_MAX_POWER || abs(actuateright) > MOTOR_MAX_POWER)){ fwdIstate += errfwd; rotIstate += errtheta; } } else { mleft(0); mright(0); fwdIstate = 0; rotIstate = 0; } //mfwdpowdbg = 0;//fwdfiltstate;//; //mrotpowdbg = rotIstate*Igain_rot;//thetafiltstate;//; } }