This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Processes/AI/ai.cpp@68:662164480f60, 2013-04-14 (annotated)
- Committer:
- rsavitski
- Date:
- Sun Apr 14 18:58:33 2013 +0000
- Revision:
- 68:662164480f60
- Parent:
- 65:4709ff6c753c
- Child:
- 69:4b7bb92916da
merge point
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; |
rsavitski | 54:99d3158c9207 | 19 | current_waypoint.theta = PI; |
rsavitski | 54:99d3158c9207 | 20 | current_waypoint.pos_threshold = 0.01; |
rsavitski | 65:4709ff6c753c | 21 | current_waypoint.angle_threshold = 0.01*PI; |
rsavitski | 39:44d3dea4adcc | 22 | |
rsavitski | 57:d434ceab6892 | 23 | motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); |
rsavitski | 39:44d3dea4adcc | 24 | |
madcowswe | 50:937e860f4621 | 25 | Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER); |
madcowswe | 50:937e860f4621 | 26 | Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER); |
rsavitski | 55:0c8897da6b3a | 27 | |
rsavitski | 68:662164480f60 | 28 | float r = 0.61+0.02+0.01; |
rsavitski | 53:b013df99b747 | 29 | |
rsavitski | 54:99d3158c9207 | 30 | for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;) |
rsavitski | 30:791739422122 | 31 | { |
rsavitski | 39:44d3dea4adcc | 32 | motion::waypoint_flag_mutex.lock(); |
rsavitski | 65:4709ff6c753c | 33 | if (motion::checkMotionStatus() && c_upper.getColour()==RED) |
rsavitski | 30:791739422122 | 34 | { |
rsavitski | 65:4709ff6c753c | 35 | //temphack!!!!! |
rsavitski | 65:4709ff6c753c | 36 | Thread::wait(1000); |
rsavitski | 65:4709ff6c753c | 37 | arm::upper_arm.go_down(); |
rsavitski | 65:4709ff6c753c | 38 | Thread::wait(2000); |
rsavitski | 65:4709ff6c753c | 39 | arm::upper_arm.go_up(); |
rsavitski | 65:4709ff6c753c | 40 | |
rsavitski | 54:99d3158c9207 | 41 | phi -= 22.5/180*PI; |
rsavitski | 54:99d3158c9207 | 42 | current_waypoint.x = 1.5-r*cos(phi); |
rsavitski | 54:99d3158c9207 | 43 | current_waypoint.y = 2-r*sin(phi); |
rsavitski | 54:99d3158c9207 | 44 | current_waypoint.theta = constrainAngle(phi+PI/2); |
rsavitski | 56:ed585a82092b | 45 | |
rsavitski | 56:ed585a82092b | 46 | //arm offset |
rsavitski | 56:ed585a82092b | 47 | current_waypoint.x += 0.0425*cos(current_waypoint.theta); |
rsavitski | 56:ed585a82092b | 48 | current_waypoint.y += 0.0425*sin(current_waypoint.theta); |
rsavitski | 57:d434ceab6892 | 49 | |
rsavitski | 57:d434ceab6892 | 50 | motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); |
rsavitski | 30:791739422122 | 51 | } |
rsavitski | 54:99d3158c9207 | 52 | motion::waypoint_flag_mutex.unlock(); |
rsavitski | 54:99d3158c9207 | 53 | |
rsavitski | 54:99d3158c9207 | 54 | Thread::wait(50); |
rsavitski | 30:791739422122 | 55 | } |
rsavitski | 30:791739422122 | 56 | } |
rsavitski | 30:791739422122 | 57 | |
rsavitski | 30:791739422122 | 58 | } //namespace |