2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/AI/ai.cpp
- Revision:
- 55:0c8897da6b3a
- Parent:
- 50:937e860f4621
- Parent:
- 54:99d3158c9207
- Child:
- 56:ed585a82092b
diff -r bc261eae004b -r 0c8897da6b3a Processes/AI/ai.cpp --- 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); } }