This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Diff: Processes/AI/ai.cpp
- Revision:
- 46:adcd57a5e402
- Parent:
- 39:44d3dea4adcc
- Child:
- 50:937e860f4621
--- a/Processes/AI/ai.cpp Fri Apr 12 20:40:52 2013 +0000 +++ b/Processes/AI/ai.cpp Fri Apr 12 20:59:18 2013 +0000 @@ -23,18 +23,24 @@ */ current_waypoint[0].x = 0.5; - current_waypoint[0].y = 1.85; + current_waypoint[0].y = 1.65; 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.45; + current_waypoint[1].theta = 0.0; + current_waypoint[1].pos_threshold = 0.05; + current_waypoint[1].angle_threshold = 0.05*PI; - current_waypoint[1].x = 1.2; - current_waypoint[1].y = 0.18; - current_waypoint[1].theta = 0; - current_waypoint[1].pos_threshold = 0.01; - current_waypoint[1].angle_threshold = 0.00001; + current_waypoint[2].x = 1.2; + current_waypoint[2].y = 0.18; + current_waypoint[2].theta = 0; + current_waypoint[2].pos_threshold = 0.01; + current_waypoint[2].angle_threshold = 0.00001; - current_waypoint[2].x = -999; + current_waypoint[3].x = -999; /* //TODO: temp current waypoint hack current_waypoint = new Waypoint; @@ -52,15 +58,19 @@ secondwp->angle_threshold = 0.00001; */ motion::setNewWaypoint(current_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) { + Thread::wait(50); + motion::waypoint_flag_mutex.lock(); if (motion::checkWaypointStatus()) { - if ((current_waypoint+1)->x != -999) + if (((current_waypoint+1)->x != -999) && ((c_upper.getColour()==BLUE) || (c_lower.getColour()==RED))) { motion::clearWaypointReached(); current_waypoint++;