This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Processes/AI/ai.cpp@55:0c8897da6b3a, 2013-04-12 (annotated)
- Committer:
- rsavitski
- Date:
- Fri Apr 12 22:00:32 2013 +0000
- Revision:
- 55:0c8897da6b3a
- Parent:
- 50:937e860f4621
- Parent:
- 54:99d3158c9207
- Child:
- 56:ed585a82092b
three way merge done. moves around cake and looks for colours
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rsavitski | 30:791739422122 | 1 | #include "ai.h" |
rsavitski | 55:0c8897da6b3a | 2 | #include "rtos.h" |
rsavitski | 55:0c8897da6b3a | 3 | #include "globals.h" |
rsavitski | 55:0c8897da6b3a | 4 | #include "motion.h" |
rsavitski | 55:0c8897da6b3a | 5 | #include "Colour.h" |
rsavitski | 55:0c8897da6b3a | 6 | #include "supportfuncs.h" |
rsavitski | 55:0c8897da6b3a | 7 | |
rsavitski | 30:791739422122 | 8 | |
rsavitski | 30:791739422122 | 9 | namespace AI |
rsavitski | 30:791739422122 | 10 | { |
rsavitski | 30:791739422122 | 11 | |
rsavitski | 30:791739422122 | 12 | void ailayer(void const *dummy) |
rsavitski | 30:791739422122 | 13 | { |
rsavitski | 54:99d3158c9207 | 14 | Waypoint current_waypoint; |
rsavitski | 38:c9058a401410 | 15 | |
rsavitski | 54:99d3158c9207 | 16 | current_waypoint.x = 2.2; |
rsavitski | 54:99d3158c9207 | 17 | current_waypoint.y = 1.85; |
rsavitski | 54:99d3158c9207 | 18 | current_waypoint.theta = PI; |
rsavitski | 54:99d3158c9207 | 19 | current_waypoint.pos_threshold = 0.01; |
rsavitski | 54:99d3158c9207 | 20 | current_waypoint.angle_threshold = 0.02*PI; |
rsavitski | 39:44d3dea4adcc | 21 | |
rsavitski | 54:99d3158c9207 | 22 | motion::setNewWaypoint(¤t_waypoint); |
rsavitski | 39:44d3dea4adcc | 23 | |
madcowswe | 50:937e860f4621 | 24 | Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER); |
madcowswe | 50:937e860f4621 | 25 | Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER); |
rsavitski | 55:0c8897da6b3a | 26 | |
rsavitski | 55:0c8897da6b3a | 27 | float r = 0.61+0.02; |
rsavitski | 53:b013df99b747 | 28 | |
rsavitski | 54:99d3158c9207 | 29 | for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;) |
rsavitski | 30:791739422122 | 30 | { |
rsavitski | 39:44d3dea4adcc | 31 | motion::waypoint_flag_mutex.lock(); |
rsavitski | 55:0c8897da6b3a | 32 | if (motion::checkWaypointStatus() && c_upper.getColour()==RED) |
rsavitski | 30:791739422122 | 33 | { |
rsavitski | 54:99d3158c9207 | 34 | phi -= 22.5/180*PI; |
rsavitski | 54:99d3158c9207 | 35 | current_waypoint.x = 1.5-r*cos(phi); |
rsavitski | 54:99d3158c9207 | 36 | current_waypoint.y = 2-r*sin(phi); |
rsavitski | 54:99d3158c9207 | 37 | current_waypoint.theta = constrainAngle(phi+PI/2); |
rsavitski | 54:99d3158c9207 | 38 | motion::clearWaypointReached(); |
rsavitski | 54:99d3158c9207 | 39 | motion::setNewWaypoint(¤t_waypoint); |
rsavitski | 30:791739422122 | 40 | } |
rsavitski | 54:99d3158c9207 | 41 | motion::waypoint_flag_mutex.unlock(); |
rsavitski | 54:99d3158c9207 | 42 | |
rsavitski | 54:99d3158c9207 | 43 | Thread::wait(50); |
rsavitski | 30:791739422122 | 44 | } |
rsavitski | 30:791739422122 | 45 | } |
rsavitski | 30:791739422122 | 46 | |
rsavitski | 30:791739422122 | 47 | } //namespace |