This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Processes/AI/ai.cpp@67:be3ea5450cc7, 2013-04-14 (annotated)
- Committer:
- madcowswe
- Date:
- Sun Apr 14 18:47:17 2013 +0000
- Revision:
- 67:be3ea5450cc7
- Parent:
- 66:f1d75e51398d
- Parent:
- 65:4709ff6c753c
- Child:
- 69:4b7bb92916da
Tuned merge. Testing needed
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 | 65:4709ff6c753c | 7 | #include "Arm.h" |
rsavitski | 55:0c8897da6b3a | 8 | |
rsavitski | 30:791739422122 | 9 | |
rsavitski | 30:791739422122 | 10 | namespace AI |
rsavitski | 30:791739422122 | 11 | { |
rsavitski | 30:791739422122 | 12 | |
rsavitski | 30:791739422122 | 13 | void ailayer(void const *dummy) |
rsavitski | 30:791739422122 | 14 | { |
rsavitski | 54:99d3158c9207 | 15 | Waypoint current_waypoint; |
rsavitski | 38:c9058a401410 | 16 | |
rsavitski | 54:99d3158c9207 | 17 | current_waypoint.x = 2.2; |
rsavitski | 54:99d3158c9207 | 18 | current_waypoint.y = 1.85; |
madcowswe | 67:be3ea5450cc7 | 19 | current_waypoint.theta = (-3.0f/4.0f)*PI; |
madcowswe | 67:be3ea5450cc7 | 20 | current_waypoint.pos_threshold = 0.03; |
rsavitski | 54:99d3158c9207 | 21 | current_waypoint.angle_threshold = 0.02*PI; |
rsavitski | 39:44d3dea4adcc | 22 | |
rsavitski | 57:d434ceab6892 | 23 | motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); |
madcowswe | 66:f1d75e51398d | 24 | motion::collavoiden = 1; |
rsavitski | 39:44d3dea4adcc | 25 | |
madcowswe | 50:937e860f4621 | 26 | Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER); |
madcowswe | 50:937e860f4621 | 27 | Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER); |
rsavitski | 55:0c8897da6b3a | 28 | |
madcowswe | 67:be3ea5450cc7 | 29 | float r = 0.61+0.02+0.01; |
rsavitski | 53:b013df99b747 | 30 | |
madcowswe | 66:f1d75e51398d | 31 | bool firstavoidstop = 1; |
rsavitski | 54:99d3158c9207 | 32 | for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;) |
rsavitski | 30:791739422122 | 33 | { |
rsavitski | 39:44d3dea4adcc | 34 | motion::waypoint_flag_mutex.lock(); |
madcowswe | 67:be3ea5450cc7 | 35 | if (motion::checkMotionStatus() && (c_upper.getColour()==RED || firstavoidstop)) |
rsavitski | 30:791739422122 | 36 | { |
rsavitski | 65:4709ff6c753c | 37 | //temphack!!!!! |
rsavitski | 65:4709ff6c753c | 38 | Thread::wait(1000); |
rsavitski | 65:4709ff6c753c | 39 | arm::upper_arm.go_down(); |
rsavitski | 65:4709ff6c753c | 40 | Thread::wait(2000); |
rsavitski | 65:4709ff6c753c | 41 | arm::upper_arm.go_up(); |
rsavitski | 65:4709ff6c753c | 42 | |
rsavitski | 54:99d3158c9207 | 43 | phi -= 22.5/180*PI; |
rsavitski | 54:99d3158c9207 | 44 | current_waypoint.x = 1.5-r*cos(phi); |
rsavitski | 54:99d3158c9207 | 45 | current_waypoint.y = 2-r*sin(phi); |
rsavitski | 54:99d3158c9207 | 46 | current_waypoint.theta = constrainAngle(phi+PI/2); |
rsavitski | 56:ed585a82092b | 47 | |
rsavitski | 56:ed585a82092b | 48 | //arm offset |
rsavitski | 56:ed585a82092b | 49 | current_waypoint.x += 0.0425*cos(current_waypoint.theta); |
rsavitski | 56:ed585a82092b | 50 | current_waypoint.y += 0.0425*sin(current_waypoint.theta); |
madcowswe | 67:be3ea5450cc7 | 51 | |
madcowswe | 67:be3ea5450cc7 | 52 | current_waypoint.pos_threshold = 0.01; |
madcowswe | 67:be3ea5450cc7 | 53 | current_waypoint.angle_threshold = 0.01*PI; |
rsavitski | 57:d434ceab6892 | 54 | |
rsavitski | 57:d434ceab6892 | 55 | motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); |
madcowswe | 66:f1d75e51398d | 56 | if (firstavoidstop){ |
madcowswe | 66:f1d75e51398d | 57 | motion::collavoiden = 0; |
madcowswe | 66:f1d75e51398d | 58 | firstavoidstop = 0; |
madcowswe | 66:f1d75e51398d | 59 | } |
madcowswe | 66:f1d75e51398d | 60 | else |
madcowswe | 66:f1d75e51398d | 61 | motion::collavoiden = 1; |
rsavitski | 30:791739422122 | 62 | } |
rsavitski | 54:99d3158c9207 | 63 | motion::waypoint_flag_mutex.unlock(); |
rsavitski | 54:99d3158c9207 | 64 | |
rsavitski | 54:99d3158c9207 | 65 | Thread::wait(50); |
rsavitski | 30:791739422122 | 66 | } |
rsavitski | 30:791739422122 | 67 | } |
rsavitski | 30:791739422122 | 68 | |
rsavitski | 30:791739422122 | 69 | } //namespace |