Colour sensors calibrated
Dependencies: mbed-rtos mbed Servo QEI
Fork of ICRSEurobot13 by
Revision 34:a49197572737, committed 2013-04-10
- Comitter:
- madcowswe
- Date:
- Wed Apr 10 19:52:19 2013 +0000
- Parent:
- 33:e3f633620816
- Child:
- 35:e1678450feec
- Child:
- 36:f8e7f0a72a3d
- Commit message:
- untuned navigation half working
Changed in this revision
--- a/Processes/AI/ai.cpp Wed Apr 10 18:25:16 2013 +0000 +++ b/Processes/AI/ai.cpp Wed Apr 10 19:52:19 2013 +0000 @@ -40,8 +40,6 @@ } waypoint_flag_mutex.unlock(); } - - Thread::yield(); //TODO!!!!!!!!!!!!!!!!!!! } void setWaypointReached()
--- a/Processes/Kalman/Kalman.h Wed Apr 10 18:25:16 2013 +0000 +++ b/Processes/Kalman/Kalman.h Wed Apr 10 19:52:19 2013 +0000 @@ -7,13 +7,6 @@ namespace Kalman { -typedef struct State -{ - float x; - float y; - float theta; -} State; - //Accessor function to get the state as one consistent struct State getState();
--- a/Processes/Motion/motion.cpp Wed Apr 10 18:25:16 2013 +0000 +++ b/Processes/Motion/motion.cpp Wed Apr 10 19:52:19 2013 +0000 @@ -16,7 +16,6 @@ Waypoint target_waypoint = *AI::current_waypoint; // get current state from Kalman - using Kalman::State; State current_state = Kalman::getState(); float delta_x = target_waypoint.x - current_state.x; @@ -32,11 +31,14 @@ if (distance_err < target_waypoint.pos_threshold) { distance_err = 0; + angle_err = constrainAngle(target_waypoint.theta - current_state.theta); + + if (abs(angle_err) < target_waypoint.angle_threshold) + { + angle_err = 0; + } } - if (abs(angle_err) < target_waypoint.angle_threshold) - { - angle_err = 0; - } + AI::waypoint_flag_mutex.lock(); // proper way would be to construct a function to evaluate the condition and pass the function pointer to a conditional setter function for reached flag if (distance_err == 0 && angle_err == 0) @@ -47,9 +49,9 @@ AI::waypoint_flag_mutex.unlock(); // angular velocity controller - const float p_gain_av = 0.3; //TODO: tune + const float p_gain_av = 0.8; //TODO: tune - const float max_av = 0.2*PI; // radians per sec //TODO: tune + const float max_av = 0.5*PI; // radians per sec //TODO: tune // angle error [-pi, pi] float angular_v = p_gain_av * angle_err; @@ -62,7 +64,7 @@ // forward velocity controller - const float p_gain_fv = 0.3; //TODO: tune + const float p_gain_fv = 0.8; //TODO: tune float max_fv = 0.2; // meters per sec //TODO: tune const float angle_envelope_exponent = 8.0; //TODO: tune
--- 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)); }
--- a/globals.cpp Wed Apr 10 18:25:16 2013 +0000 +++ b/globals.cpp Wed Apr 10 19:52:19 2013 +0000 @@ -3,4 +3,3 @@ //Store global objects here pos beaconpos[] = {{-0.040,1}, {3.040,-0.040}, {3.040,2.040}}; -Timer SystemTime;
--- a/globals.h Wed Apr 10 18:25:16 2013 +0000 +++ b/globals.h Wed Apr 10 19:52:19 2013 +0000 @@ -19,7 +19,7 @@ const float angvarpertime = 0;//0.001; const float MOTORCONTROLLER_FILTER_K = 0.5;// TODO: tune this -const float MOTOR_MAX_POWER = 0.4f; +const float MOTOR_MAX_POWER = 0.5f; /* PINOUT Sensors @@ -106,4 +106,11 @@ float angle_threshold; } Waypoint; +typedef struct State +{ + float x; + float y; + float theta; +} State; + #endif //GLOBALS_H \ No newline at end of file