This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Processes/AI/ai.cpp@46:adcd57a5e402, 2013-04-12 (annotated)
- Committer:
- xiaxia686
- Date:
- Fri Apr 12 20:59:18 2013 +0000
- Revision:
- 46:adcd57a5e402
- Parent:
- 39:44d3dea4adcc
- Child:
- 50:937e860f4621
Colours Sensors fixed
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rsavitski | 30:791739422122 | 1 | #include "ai.h" |
rsavitski | 30:791739422122 | 2 | |
rsavitski | 30:791739422122 | 3 | namespace AI |
rsavitski | 30:791739422122 | 4 | { |
rsavitski | 30:791739422122 | 5 | |
rsavitski | 30:791739422122 | 6 | void ailayer(void const *dummy) |
rsavitski | 30:791739422122 | 7 | { |
rsavitski | 39:44d3dea4adcc | 8 | Waypoint *current_waypoint = new Waypoint[5]; |
rsavitski | 39:44d3dea4adcc | 9 | /* |
rsavitski | 38:c9058a401410 | 10 | current_waypoint[0].x = 1; |
rsavitski | 38:c9058a401410 | 11 | current_waypoint[0].y = 1; |
rsavitski | 38:c9058a401410 | 12 | current_waypoint[0].theta = 0.0; |
rsavitski | 38:c9058a401410 | 13 | current_waypoint[0].pos_threshold = 0.05; |
rsavitski | 38:c9058a401410 | 14 | current_waypoint[0].angle_threshold = 0.05*PI; |
rsavitski | 38:c9058a401410 | 15 | |
rsavitski | 38:c9058a401410 | 16 | current_waypoint[1].x = 2.2; |
rsavitski | 38:c9058a401410 | 17 | current_waypoint[1].y = 1.5; |
rsavitski | 38:c9058a401410 | 18 | current_waypoint[1].theta = PI/2; |
rsavitski | 38:c9058a401410 | 19 | current_waypoint[1].pos_threshold = 0.05; |
rsavitski | 38:c9058a401410 | 20 | current_waypoint[1].angle_threshold = 0.05*PI; |
rsavitski | 38:c9058a401410 | 21 | |
rsavitski | 38:c9058a401410 | 22 | current_waypoint[2].x = -999; |
rsavitski | 39:44d3dea4adcc | 23 | */ |
rsavitski | 38:c9058a401410 | 24 | |
rsavitski | 39:44d3dea4adcc | 25 | current_waypoint[0].x = 0.5; |
xiaxia686 | 46:adcd57a5e402 | 26 | current_waypoint[0].y = 1.65; |
rsavitski | 39:44d3dea4adcc | 27 | current_waypoint[0].theta = 0.0; |
rsavitski | 39:44d3dea4adcc | 28 | current_waypoint[0].pos_threshold = 0.05; |
rsavitski | 39:44d3dea4adcc | 29 | current_waypoint[0].angle_threshold = 0.05*PI; |
xiaxia686 | 46:adcd57a5e402 | 30 | |
xiaxia686 | 46:adcd57a5e402 | 31 | current_waypoint[1].x = 2.5; |
xiaxia686 | 46:adcd57a5e402 | 32 | current_waypoint[1].y = 0.45; |
xiaxia686 | 46:adcd57a5e402 | 33 | current_waypoint[1].theta = 0.0; |
xiaxia686 | 46:adcd57a5e402 | 34 | current_waypoint[1].pos_threshold = 0.05; |
xiaxia686 | 46:adcd57a5e402 | 35 | current_waypoint[1].angle_threshold = 0.05*PI; |
rsavitski | 39:44d3dea4adcc | 36 | |
xiaxia686 | 46:adcd57a5e402 | 37 | current_waypoint[2].x = 1.2; |
xiaxia686 | 46:adcd57a5e402 | 38 | current_waypoint[2].y = 0.18; |
xiaxia686 | 46:adcd57a5e402 | 39 | current_waypoint[2].theta = 0; |
xiaxia686 | 46:adcd57a5e402 | 40 | current_waypoint[2].pos_threshold = 0.01; |
xiaxia686 | 46:adcd57a5e402 | 41 | current_waypoint[2].angle_threshold = 0.00001; |
rsavitski | 39:44d3dea4adcc | 42 | |
xiaxia686 | 46:adcd57a5e402 | 43 | current_waypoint[3].x = -999; |
rsavitski | 38:c9058a401410 | 44 | /* |
rsavitski | 30:791739422122 | 45 | //TODO: temp current waypoint hack |
rsavitski | 30:791739422122 | 46 | current_waypoint = new Waypoint; |
rsavitski | 30:791739422122 | 47 | current_waypoint->x = 0.5; |
rsavitski | 30:791739422122 | 48 | current_waypoint->y = 0.7; |
rsavitski | 30:791739422122 | 49 | current_waypoint->theta = 0.0; |
rsavitski | 30:791739422122 | 50 | current_waypoint->pos_threshold = 0.05; |
rsavitski | 38:c9058a401410 | 51 | current_waypoint->angle_threshold = 0.05*PI; |
rsavitski | 30:791739422122 | 52 | |
rsavitski | 30:791739422122 | 53 | Waypoint* secondwp = new Waypoint; |
rsavitski | 38:c9058a401410 | 54 | secondwp->x = 1.20; |
rsavitski | 38:c9058a401410 | 55 | secondwp->y = 0.18; |
rsavitski | 30:791739422122 | 56 | secondwp->theta = PI; |
rsavitski | 38:c9058a401410 | 57 | secondwp->pos_threshold = 0.01; |
rsavitski | 38:c9058a401410 | 58 | secondwp->angle_threshold = 0.00001; |
rsavitski | 38:c9058a401410 | 59 | */ |
rsavitski | 39:44d3dea4adcc | 60 | motion::setNewWaypoint(current_waypoint); |
xiaxia686 | 46:adcd57a5e402 | 61 | Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER,UPPER); |
xiaxia686 | 46:adcd57a5e402 | 62 | Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER,LOWER); |
rsavitski | 30:791739422122 | 63 | while(1) |
rsavitski | 30:791739422122 | 64 | { |
xiaxia686 | 46:adcd57a5e402 | 65 | |
rsavitski | 35:f8e7f0a72a3d | 66 | Thread::wait(50); |
rsavitski | 30:791739422122 | 67 | |
xiaxia686 | 46:adcd57a5e402 | 68 | |
rsavitski | 39:44d3dea4adcc | 69 | motion::waypoint_flag_mutex.lock(); |
rsavitski | 39:44d3dea4adcc | 70 | if (motion::checkWaypointStatus()) |
rsavitski | 30:791739422122 | 71 | { |
rsavitski | 39:44d3dea4adcc | 72 | |
xiaxia686 | 46:adcd57a5e402 | 73 | if (((current_waypoint+1)->x != -999) && ((c_upper.getColour()==BLUE) || (c_lower.getColour()==RED))) |
rsavitski | 39:44d3dea4adcc | 74 | { |
rsavitski | 39:44d3dea4adcc | 75 | motion::clearWaypointReached(); |
rsavitski | 38:c9058a401410 | 76 | current_waypoint++; |
rsavitski | 39:44d3dea4adcc | 77 | motion::setNewWaypoint(current_waypoint); |
rsavitski | 39:44d3dea4adcc | 78 | } |
rsavitski | 30:791739422122 | 79 | } |
rsavitski | 39:44d3dea4adcc | 80 | motion::waypoint_flag_mutex.unlock(); |
rsavitski | 30:791739422122 | 81 | } |
rsavitski | 30:791739422122 | 82 | } |
rsavitski | 30:791739422122 | 83 | |
rsavitski | 30:791739422122 | 84 | } //namespace |