2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Processes/AI/ai.cpp@53:b013df99b747, 2013-04-12 (annotated)
- Committer:
- rsavitski
- Date:
- Fri Apr 12 18:22:53 2013 +0000
- Revision:
- 53:b013df99b747
- Parent:
- 52:bffe5f7c39a3
- Child:
- 54:99d3158c9207
waypoints with reversing, motion API still hacky
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 | 53:b013df99b747 | 25 | current_waypoint[0].x = 1.8; |
rsavitski | 53:b013df99b747 | 26 | current_waypoint[0].y = 1.39; |
rsavitski | 52:bffe5f7c39a3 | 27 | current_waypoint[0].theta = PI; |
rsavitski | 53:b013df99b747 | 28 | current_waypoint[0].pos_threshold = 0.03; |
rsavitski | 53:b013df99b747 | 29 | current_waypoint[0].angle_threshold = 0.05*PI; |
rsavitski | 39:44d3dea4adcc | 30 | |
rsavitski | 53:b013df99b747 | 31 | current_waypoint[1].x = 1.5; |
rsavitski | 53:b013df99b747 | 32 | current_waypoint[1].y = 1.39; |
rsavitski | 53:b013df99b747 | 33 | current_waypoint[1].theta = PI; |
rsavitski | 53:b013df99b747 | 34 | current_waypoint[1].pos_threshold = 0.03; |
rsavitski | 53:b013df99b747 | 35 | current_waypoint[1].angle_threshold = 0.05*PI; |
rsavitski | 39:44d3dea4adcc | 36 | |
rsavitski | 53:b013df99b747 | 37 | current_waypoint[2].x = -999; |
rsavitski | 38:c9058a401410 | 38 | /* |
rsavitski | 30:791739422122 | 39 | //TODO: temp current waypoint hack |
rsavitski | 30:791739422122 | 40 | current_waypoint = new Waypoint; |
rsavitski | 30:791739422122 | 41 | current_waypoint->x = 0.5; |
rsavitski | 30:791739422122 | 42 | current_waypoint->y = 0.7; |
rsavitski | 30:791739422122 | 43 | current_waypoint->theta = 0.0; |
rsavitski | 30:791739422122 | 44 | current_waypoint->pos_threshold = 0.05; |
rsavitski | 38:c9058a401410 | 45 | current_waypoint->angle_threshold = 0.05*PI; |
rsavitski | 30:791739422122 | 46 | |
rsavitski | 30:791739422122 | 47 | Waypoint* secondwp = new Waypoint; |
rsavitski | 38:c9058a401410 | 48 | secondwp->x = 1.20; |
rsavitski | 38:c9058a401410 | 49 | secondwp->y = 0.18; |
rsavitski | 30:791739422122 | 50 | secondwp->theta = PI; |
rsavitski | 38:c9058a401410 | 51 | secondwp->pos_threshold = 0.01; |
rsavitski | 38:c9058a401410 | 52 | secondwp->angle_threshold = 0.00001; |
rsavitski | 38:c9058a401410 | 53 | */ |
rsavitski | 53:b013df99b747 | 54 | |
rsavitski | 39:44d3dea4adcc | 55 | motion::setNewWaypoint(current_waypoint); |
rsavitski | 30:791739422122 | 56 | while(1) |
rsavitski | 30:791739422122 | 57 | { |
rsavitski | 35:f8e7f0a72a3d | 58 | Thread::wait(50); |
rsavitski | 30:791739422122 | 59 | |
rsavitski | 39:44d3dea4adcc | 60 | motion::waypoint_flag_mutex.lock(); |
rsavitski | 39:44d3dea4adcc | 61 | if (motion::checkWaypointStatus()) |
rsavitski | 30:791739422122 | 62 | { |
rsavitski | 39:44d3dea4adcc | 63 | |
rsavitski | 39:44d3dea4adcc | 64 | if ((current_waypoint+1)->x != -999) |
rsavitski | 39:44d3dea4adcc | 65 | { |
rsavitski | 39:44d3dea4adcc | 66 | motion::clearWaypointReached(); |
rsavitski | 38:c9058a401410 | 67 | current_waypoint++; |
rsavitski | 39:44d3dea4adcc | 68 | motion::setNewWaypoint(current_waypoint); |
rsavitski | 39:44d3dea4adcc | 69 | } |
rsavitski | 30:791739422122 | 70 | } |
rsavitski | 39:44d3dea4adcc | 71 | motion::waypoint_flag_mutex.unlock(); |
rsavitski | 30:791739422122 | 72 | } |
rsavitski | 30:791739422122 | 73 | } |
rsavitski | 30:791739422122 | 74 | |
rsavitski | 30:791739422122 | 75 | } //namespace |