2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Processes/AI/ai.cpp@57:d434ceab6892, 2013-04-13 (annotated)
- Committer:
- rsavitski
- Date:
- Sat Apr 13 21:29:35 2013 +0000
- Revision:
- 57:d434ceab6892
- Parent:
- 56:ed585a82092b
- Child:
- 65:4709ff6c753c
- Child:
- 66:f1d75e51398d
refactored motion with proxy movement object, working
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 | 57:d434ceab6892 | 22 | motion::setNewWaypoint(Thread::gettid(),¤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 | 57:d434ceab6892 | 32 | if (motion::checkMotionStatus() && c_lower.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 | 56:ed585a82092b | 38 | |
rsavitski | 56:ed585a82092b | 39 | //arm offset |
rsavitski | 56:ed585a82092b | 40 | current_waypoint.x += 0.0425*cos(current_waypoint.theta); |
rsavitski | 56:ed585a82092b | 41 | current_waypoint.y += 0.0425*sin(current_waypoint.theta); |
rsavitski | 57:d434ceab6892 | 42 | |
rsavitski | 57:d434ceab6892 | 43 | motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); |
rsavitski | 30:791739422122 | 44 | } |
rsavitski | 54:99d3158c9207 | 45 | motion::waypoint_flag_mutex.unlock(); |
rsavitski | 54:99d3158c9207 | 46 | |
rsavitski | 54:99d3158c9207 | 47 | Thread::wait(50); |
rsavitski | 30:791739422122 | 48 | } |
rsavitski | 30:791739422122 | 49 | } |
rsavitski | 30:791739422122 | 50 | |
rsavitski | 30:791739422122 | 51 | } //namespace |