2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Processes/AI/ai.cpp@33:a49197572737, 2013-04-10 (annotated)
- Committer:
- madcowswe
- Date:
- Wed Apr 10 19:52:19 2013 +0000
- Revision:
- 33:a49197572737
- Parent:
- 30:791739422122
- Child:
- 35:f8e7f0a72a3d
untuned navigation half working
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 | Mutex waypoint_flag_mutex; |
rsavitski | 30:791739422122 | 7 | |
rsavitski | 30:791739422122 | 8 | Waypoint* current_waypoint = NULL; //global scope |
rsavitski | 30:791739422122 | 9 | |
rsavitski | 30:791739422122 | 10 | bool waypoint_reached = false; // is current waypoint reached |
rsavitski | 30:791739422122 | 11 | |
rsavitski | 30:791739422122 | 12 | void ailayer(void const *dummy) |
rsavitski | 30:791739422122 | 13 | { |
rsavitski | 30:791739422122 | 14 | //TODO: temp current waypoint hack |
rsavitski | 30:791739422122 | 15 | current_waypoint = new Waypoint; |
rsavitski | 30:791739422122 | 16 | current_waypoint->x = 0.5; |
rsavitski | 30:791739422122 | 17 | current_waypoint->y = 0.7; |
rsavitski | 30:791739422122 | 18 | current_waypoint->theta = 0.0; |
rsavitski | 30:791739422122 | 19 | current_waypoint->pos_threshold = 0.05; |
rsavitski | 30:791739422122 | 20 | current_waypoint->angle_threshold = 0.1*PI; |
rsavitski | 30:791739422122 | 21 | |
rsavitski | 30:791739422122 | 22 | Waypoint* secondwp = new Waypoint; |
rsavitski | 30:791739422122 | 23 | secondwp->x = 2; |
rsavitski | 30:791739422122 | 24 | secondwp->y = 1.2; |
rsavitski | 30:791739422122 | 25 | secondwp->theta = PI; |
rsavitski | 30:791739422122 | 26 | secondwp->pos_threshold = 0.05; |
rsavitski | 30:791739422122 | 27 | secondwp->angle_threshold = 0.1*PI; |
rsavitski | 30:791739422122 | 28 | |
rsavitski | 30:791739422122 | 29 | |
rsavitski | 30:791739422122 | 30 | while(1) |
rsavitski | 30:791739422122 | 31 | { |
rsavitski | 30:791739422122 | 32 | Thread::wait(100); |
rsavitski | 30:791739422122 | 33 | |
rsavitski | 30:791739422122 | 34 | waypoint_flag_mutex.lock(); |
rsavitski | 30:791739422122 | 35 | if (checkWaypointStatus()) |
rsavitski | 30:791739422122 | 36 | { |
rsavitski | 30:791739422122 | 37 | clearWaypointReached(); |
rsavitski | 30:791739422122 | 38 | delete current_waypoint; |
rsavitski | 30:791739422122 | 39 | current_waypoint = secondwp; |
rsavitski | 30:791739422122 | 40 | } |
rsavitski | 30:791739422122 | 41 | waypoint_flag_mutex.unlock(); |
rsavitski | 30:791739422122 | 42 | } |
rsavitski | 30:791739422122 | 43 | } |
rsavitski | 30:791739422122 | 44 | |
rsavitski | 30:791739422122 | 45 | void setWaypointReached() |
rsavitski | 30:791739422122 | 46 | { |
rsavitski | 30:791739422122 | 47 | waypoint_reached = true; |
rsavitski | 30:791739422122 | 48 | } |
rsavitski | 30:791739422122 | 49 | |
rsavitski | 30:791739422122 | 50 | void clearWaypointReached() |
rsavitski | 30:791739422122 | 51 | { |
rsavitski | 30:791739422122 | 52 | waypoint_reached = false; |
rsavitski | 30:791739422122 | 53 | } |
rsavitski | 30:791739422122 | 54 | |
rsavitski | 30:791739422122 | 55 | bool checkWaypointStatus() |
rsavitski | 30:791739422122 | 56 | { |
rsavitski | 30:791739422122 | 57 | return waypoint_reached; |
rsavitski | 30:791739422122 | 58 | } |
rsavitski | 30:791739422122 | 59 | |
rsavitski | 30:791739422122 | 60 | } //namespace |