2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/AI/ai.cpp
- Revision:
- 49:665bdca0f2cd
- Parent:
- 39:44d3dea4adcc
- Child:
- 50:937e860f4621
--- a/Processes/AI/ai.cpp Fri Apr 12 17:03:53 2013 +0000 +++ b/Processes/AI/ai.cpp Fri Apr 12 21:07:00 2013 +0000 @@ -23,16 +23,16 @@ */ current_waypoint[0].x = 0.5; - current_waypoint[0].y = 1.85; + 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 = 1.2; - current_waypoint[1].y = 0.18; + current_waypoint[1].x = 2.5; + current_waypoint[1].y = 0.5; current_waypoint[1].theta = 0; - current_waypoint[1].pos_threshold = 0.01; - current_waypoint[1].angle_threshold = 0.00001; + current_waypoint[1].pos_threshold = 0.05; + current_waypoint[1].angle_threshold = 0.05*PI; current_waypoint[2].x = -999; /* @@ -52,6 +52,8 @@ secondwp->angle_threshold = 0.00001; */ motion::setNewWaypoint(current_waypoint); + + int currwptidx = 0; while(1) { Thread::wait(50); @@ -60,12 +62,8 @@ if (motion::checkWaypointStatus()) { - if ((current_waypoint+1)->x != -999) - { - motion::clearWaypointReached(); - current_waypoint++; - motion::setNewWaypoint(current_waypoint); - } + motion::setNewWaypoint(¤t_waypoint[++currwptidx % 2]); + motion::clearWaypointReached(); } motion::waypoint_flag_mutex.unlock(); }