2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Revision 62:78d99b781f02, committed 2013-04-14
- Comitter:
- madcowswe
- Date:
- Sun Apr 14 12:57:04 2013 +0000
- Parent:
- 61:a7782a35ce4f
- Child:
- 63:c2c6269767b8
- Commit message:
- Basic fwd dc power feed forward working
Changed in this revision
--- a/Processes/MotorControl/MotorControl.cpp Sun Apr 14 10:49:51 2013 +0000 +++ b/Processes/MotorControl/MotorControl.cpp Sun Apr 14 12:57:04 2013 +0000 @@ -4,6 +4,7 @@ #include "globals.h" #include <algorithm> #include "system.h" +#include "supportfuncs.h" namespace MotorControl { @@ -11,12 +12,16 @@ 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() { - - 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; + const float power_per_dc_m_per_s = 1.64f; + const float hysteresis_pwr = 0.16f; float testspeed = 0.2; float Fcrit = 1.75; @@ -48,6 +53,7 @@ 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)); @@ -61,7 +67,7 @@ fwdIstate += errfwd; rotIstate += errtheta; - float actuatefwd = errfwd*Pgain + fwdIstate*Igain + power_per_dc_m_per_s*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); @@ -69,6 +75,9 @@ mleft(max(min(actuateleft, MOTOR_MAX_POWER), -MOTOR_MAX_POWER)); mright(max(min(actuateright, MOTOR_MAX_POWER), -MOTOR_MAX_POWER)); + + mfwdpowdbg = fwdIstate*Igain; + mrotpowdbg = rotIstate*Igain_rot; }
--- a/Processes/MotorControl/MotorControl.h Sun Apr 14 10:49:51 2013 +0000 +++ b/Processes/MotorControl/MotorControl.h Sun Apr 14 12:57:04 2013 +0000 @@ -4,8 +4,11 @@ namespace MotorControl{ - extern float fwdcmd; - extern float omegacmd; + extern volatile float fwdcmd; + extern volatile float omegacmd; + + extern volatile float mfwdpowdbg; + extern volatile float mrotpowdbg; inline void set_fwdcmd(float infwdcmd){ fwdcmd = infwdcmd;
--- a/Processes/Printing/Printing.h Sun Apr 14 10:49:51 2013 +0000 +++ b/Processes/Printing/Printing.h Sun Apr 14 12:57:04 2013 +0000 @@ -1,7 +1,7 @@ // Eurobot13 Printing.h -//#define PRINTINGOFF +#define PRINTINGOFF #include "mbed.h" #include "rtos.h"
--- a/globals.h Sun Apr 14 10:49:51 2013 +0000 +++ b/globals.h Sun Apr 14 12:57:04 2013 +0000 @@ -8,7 +8,7 @@ #define ENABLE_GLOBAL_ENCODERS -const float ENCODER_M_PER_TICK = 1.0f/11980.0f; +const float ENCODER_M_PER_TICK = 1.0f/1198.0f; const float ENCODER_WHEELBASE = 0.068; const float TURRET_FWD_PLACEMENT = -0.042;
--- a/main.cpp Sun Apr 14 10:49:51 2013 +0000 +++ b/main.cpp Sun Apr 14 12:57:04 2013 +0000 @@ -73,19 +73,34 @@ Ticker motorcontroltestticker; motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05); // ai layer thread - Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048); + //Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048); // motion layer periodic callback - RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic); - motion_timer.start(50); + //RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic); + //motion_timer.start(50); - Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048); + //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 + + MotorControl::set_omegacmd(0); + for(float spd = 0.02; spd <= 0.5; spd *= 1.4){ + + MotorControl::set_fwdcmd(spd); + + Thread::wait(3000); + + float f = MotorControl::mfwdpowdbg; + float r = MotorControl::mrotpowdbg; + MotorControl::set_fwdcmd(0); + printf("targetspd:%f, fwd:%f, rot:%f\r\n", spd, f, r); + Thread::wait(5000); + } + Thread::wait(osWaitForever); }
--- a/supportfuncs.h Sun Apr 14 10:49:51 2013 +0000 +++ b/supportfuncs.h Sun Apr 14 12:57:04 2013 +0000 @@ -5,6 +5,10 @@ #include "globals.h" #include "tvmet/Matrix.h" +template <typename T> int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + //Constrains agles to +/- pi inline float constrainAngle(float x){ x = fmod(x + PI, 2*PI);