2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Revision 55:0c8897da6b3a, committed 2013-04-12
- Comitter:
- rsavitski
- Date:
- Fri Apr 12 22:00:32 2013 +0000
- Parent:
- 51:bc261eae004b
- Parent:
- 54:99d3158c9207
- Child:
- 56:ed585a82092b
- Child:
- 60:5058465904e0
- Commit message:
- three way merge done. moves around cake and looks for colours
Changed in this revision
Processes/AI/ai.cpp | Show annotated file Show diff for this revision Revisions of this file |
Processes/AI/ai.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/Processes/AI/ai.cpp Fri Apr 12 21:34:34 2013 +0000 +++ b/Processes/AI/ai.cpp Fri Apr 12 22:00:32 2013 +0000 @@ -1,44 +1,46 @@ #include "ai.h" +#include "rtos.h" +#include "globals.h" +#include "motion.h" +#include "Colour.h" +#include "supportfuncs.h" + namespace AI { void ailayer(void const *dummy) { - Waypoint *current_waypoint = new Waypoint[5]; + Waypoint current_waypoint; - current_waypoint[0].x = 0.5; - current_waypoint[0].y = 0.5; - current_waypoint[0].theta = 0.0; - current_waypoint[0].pos_threshold = 0.05; - current_waypoint[0].angle_threshold = 0.05*PI; - - current_waypoint[1].x = 2.5; - current_waypoint[1].y = 0.5; - current_waypoint[1].theta = 0; - current_waypoint[1].pos_threshold = 0.05; - current_waypoint[1].angle_threshold = 0.05*PI; + current_waypoint.x = 2.2; + current_waypoint.y = 1.85; + current_waypoint.theta = PI; + current_waypoint.pos_threshold = 0.01; + current_waypoint.angle_threshold = 0.02*PI; - motion::setNewWaypoint(current_waypoint); - - int currwptidx = 0; + motion::setNewWaypoint(¤t_waypoint); + Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER); Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER); - - while(1) + + float r = 0.61+0.02; + + for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;) { - Thread::wait(50); - motion::waypoint_flag_mutex.lock(); - if (motion::checkWaypointStatus()) + if (motion::checkWaypointStatus() && c_upper.getColour()==RED) { - if(c_upper.getColour() == BLUE && c_lower.getColour() == RED){ - motion::setNewWaypoint(¤t_waypoint[++currwptidx % 2]); - motion::clearWaypointReached(); - } - + phi -= 22.5/180*PI; + current_waypoint.x = 1.5-r*cos(phi); + current_waypoint.y = 2-r*sin(phi); + current_waypoint.theta = constrainAngle(phi+PI/2); + motion::clearWaypointReached(); + motion::setNewWaypoint(¤t_waypoint); } - motion::waypoint_flag_mutex.unlock(); + motion::waypoint_flag_mutex.unlock(); + + Thread::wait(50); } }
--- a/Processes/AI/ai.h Fri Apr 12 21:34:34 2013 +0000 +++ b/Processes/AI/ai.h Fri Apr 12 22:00:32 2013 +0000 @@ -1,11 +1,6 @@ #ifndef EUROBOT_PROCESSES_AI_AI_H_ #define EUROBOT_PROCESSES_AI_AI_H_ -#include "rtos.h" -#include "globals.h" -#include "motion.h" -#include "Colour.h" - namespace AI {
--- a/Processes/Motion/motion.cpp Fri Apr 12 21:34:34 2013 +0000 +++ b/Processes/Motion/motion.cpp Fri Apr 12 22:00:32 2013 +0000 @@ -35,14 +35,25 @@ float angle_err = constrainAngle(atan2(delta_y, delta_x) - current_state.theta); + // reversing + bool reversing = false; + if ((abs(angle_err) > PI/2) && (distance_err < 0.2)) + { + reversing = true; + angle_err = constrainAngle(angle_err + PI); + distance_err = -distance_err; + } + + float angle_err_saved = angle_err; // actuated angle error can be overriden by the final turning code, but forward speed envelope should be controlled with the atan angle + // is the waypoint reached waypoint_flag_mutex.lock(); - if (distance_err < ((d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold)) + if (abs(distance_err) < ((d_reached) ? target_waypoint.pos_threshold+0.02 : target_waypoint.pos_threshold)) { d_reached = true; - distance_err = 0; + //distance_err = 0; - angle_err = 0.5*constrainAngle(target_waypoint.theta - current_state.theta); + angle_err = constrainAngle(target_waypoint.theta - current_state.theta); if (abs(angle_err) < target_waypoint.angle_threshold) { @@ -75,16 +86,21 @@ // forward velocity controller - const float p_gain_fv = 0.5; //TODO: tune + const float p_gain_fv = 0.25; //TODO: tune float max_fv = 0.2; // meters per sec //TODO: tune - const float angle_envelope_exponent = 8.0; //TODO: tune + float max_fv_reverse = 0.05; //TODO: tune + const float angle_envelope_exponent = 512;//32.0; //8.0; //TODO: tune // control, distance_err in meters float forward_v = p_gain_fv * distance_err; + // if reversing, robot is less stable hence a different cap is used + if (reversing) + max_fv = max_fv_reverse; + // control the forward velocity envelope based on angular error - max_fv = max_fv * pow(cos(angle_err/2), angle_envelope_exponent); + max_fv = max_fv * pow(cos(angle_err_saved/2), angle_envelope_exponent); // constrain range if (forward_v > max_fv)