This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Revision 60:5058465904e0, committed 2013-04-13
- Comitter:
- madcowswe
- Date:
- Sat Apr 13 22:41:00 2013 +0000
- Parent:
- 55:0c8897da6b3a
- Child:
- 61:a7782a35ce4f
- Commit message:
- Added feed forward. Maybe positive feedback
Changed in this revision
--- a/Processes/MotorControl/MotorControl.cpp Fri Apr 12 22:00:32 2013 +0000 +++ b/Processes/MotorControl/MotorControl.cpp Sat Apr 13 22:41:00 2013 +0000 @@ -8,13 +8,15 @@ namespace MotorControl { -float fwdcmd = 0; -float omegacmd = 0; +volatile float fwdcmd = 0; +volatile float omegacmd = 0; void motor_control_isr() { MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B); + + const float power_per_dc_m_per_s = 1.0f/0.6f; float testspeed = 0.2; float Fcrit = 1.75; @@ -51,15 +53,15 @@ 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 - omegacmd; + float errfwd = fwdcmd - fwdfiltstate; + float errtheta = omegacmd - thetafiltstate; static float fwdIstate = 0; static float rotIstate = 0; fwdIstate += errfwd; rotIstate += errtheta; - float actuatefwd = errfwd*Pgain + fwdIstate*Igain; + float actuatefwd = errfwd*Pgain + fwdIstate*Igain + power_per_dc_m_per_s*fwdcmd; float actuaterot = errtheta*Pgain_rot + rotIstate*Igain_rot; float actuateleft = actuatefwd - (actuaterot*ENCODER_WHEELBASE/2.0f);
--- a/globals.h Fri Apr 12 22:00:32 2013 +0000 +++ b/globals.h Sat Apr 13 22:41:00 2013 +0000 @@ -8,7 +8,7 @@ #define ENABLE_GLOBAL_ENCODERS -const float ENCODER_M_PER_TICK = 0.00084; +const float ENCODER_M_PER_TICK = 1.0f/11980.0f; const float ENCODER_WHEELBASE = 0.068; const float TURRET_FWD_PLACEMENT = -0.042; @@ -65,10 +65,10 @@ const PinName P_COLOR_SENSOR_IN_UPPER = p20; const PinName P_COLOR_SENSOR_IN_LOWER = p19; -const PinName P_MOT_LEFT_A = p21; -const PinName P_MOT_LEFT_B = p22; -const PinName P_MOT_RIGHT_A = p23; -const PinName P_MOT_RIGHT_B = p24; +const PinName P_MOT_LEFT_A = p22; +const PinName P_MOT_LEFT_B = p21; +const PinName P_MOT_RIGHT_A = p24; +const PinName P_MOT_RIGHT_B = p23; const PinName P_ENC_RIGHT_A = p26; const PinName P_ENC_RIGHT_B = p25;
--- a/main.cpp Fri Apr 12 22:00:32 2013 +0000 +++ b/main.cpp Sat Apr 13 22:41:00 2013 +0000 @@ -61,7 +61,6 @@ Serial pc(USBTX, USBRX); pc.baud(115200); - wait(2); InitSerial(); wait(3); Kalman::KalmanInit(); @@ -81,8 +80,12 @@ motion_timer.start(50); Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048); + + //while(1){ + // printf("r:%f, l:%f, t:%f\r\n", right_encoder.getTicks()*ENCODER_M_PER_TICK, left_encoder.getTicks()*ENCODER_M_PER_TICK, SystemTime.read()); + //} - measureCPUidle(); //repurpose thread for idle measurement + //measureCPUidle(); //repurpose thread for idle measurement Thread::wait(osWaitForever); }