This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Processes/AI/ai.cpp@74:9620d24a2f4e, 2013-04-15 (annotated)
- Committer:
- madcowswe
- Date:
- Mon Apr 15 14:31:27 2013 +0000
- Revision:
- 74:9620d24a2f4e
- Parent:
- 70:0da6ca845762
- Child:
- 78:3178a1e46146
added starting of motors
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" |
madcowswe | 74:9620d24a2f4e | 8 | #include "MotorControl.h" |
rsavitski | 55:0c8897da6b3a | 9 | |
rsavitski | 69:4b7bb92916da | 10 | //TODO: after 2013, kill entire AI layer as it is hacked together. Rest is fine-ish |
rsavitski | 30:791739422122 | 11 | |
rsavitski | 30:791739422122 | 12 | namespace AI |
rsavitski | 30:791739422122 | 13 | { |
rsavitski | 30:791739422122 | 14 | |
rsavitski | 69:4b7bb92916da | 15 | bool delayed_done = true; //TODO: kill |
rsavitski | 69:4b7bb92916da | 16 | |
rsavitski | 69:4b7bb92916da | 17 | struct delayed_struct //TODO: kill |
rsavitski | 69:4b7bb92916da | 18 | { |
rsavitski | 69:4b7bb92916da | 19 | osThreadId tid; |
rsavitski | 69:4b7bb92916da | 20 | Waypoint *wpptr; |
rsavitski | 69:4b7bb92916da | 21 | }; |
rsavitski | 69:4b7bb92916da | 22 | |
rsavitski | 70:0da6ca845762 | 23 | void arm_upper(const void *dummy); //TODO: kill |
rsavitski | 70:0da6ca845762 | 24 | |
rsavitski | 69:4b7bb92916da | 25 | void delayed_setter(const void *tid_wpptr); //TODO: kill the hack |
rsavitski | 69:4b7bb92916da | 26 | |
rsavitski | 30:791739422122 | 27 | void ailayer(void const *dummy) |
rsavitski | 30:791739422122 | 28 | { |
rsavitski | 54:99d3158c9207 | 29 | Waypoint current_waypoint; |
rsavitski | 69:4b7bb92916da | 30 | |
madcowswe | 50:937e860f4621 | 31 | Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER); |
madcowswe | 50:937e860f4621 | 32 | Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER); |
rsavitski | 55:0c8897da6b3a | 33 | |
rsavitski | 69:4b7bb92916da | 34 | // first waypoint for approach |
rsavitski | 54:99d3158c9207 | 35 | current_waypoint.x = 2.2; |
rsavitski | 54:99d3158c9207 | 36 | current_waypoint.y = 1.85; |
madcowswe | 67:be3ea5450cc7 | 37 | current_waypoint.theta = (-3.0f/4.0f)*PI; |
madcowswe | 67:be3ea5450cc7 | 38 | current_waypoint.pos_threshold = 0.03; |
rsavitski | 54:99d3158c9207 | 39 | current_waypoint.angle_threshold = 0.02*PI; |
rsavitski | 69:4b7bb92916da | 40 | |
madcowswe | 66:f1d75e51398d | 41 | motion::collavoiden = 1; |
madcowswe | 74:9620d24a2f4e | 42 | MotorControl::motorsenabled = 1; |
rsavitski | 69:4b7bb92916da | 43 | motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); |
rsavitski | 55:0c8897da6b3a | 44 | |
rsavitski | 68:662164480f60 | 45 | float r = 0.61+0.02+0.01; |
rsavitski | 53:b013df99b747 | 46 | |
madcowswe | 66:f1d75e51398d | 47 | bool firstavoidstop = 1; |
rsavitski | 69:4b7bb92916da | 48 | delayed_struct ds = {Thread::gettid(),¤t_waypoint}; |
rsavitski | 70:0da6ca845762 | 49 | RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds); |
rsavitski | 70:0da6ca845762 | 50 | RtosTimer delayed_armer(arm_upper, osTimerOnce); |
rsavitski | 69:4b7bb92916da | 51 | |
rsavitski | 54:99d3158c9207 | 52 | for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;) |
rsavitski | 30:791739422122 | 53 | { |
rsavitski | 39:44d3dea4adcc | 54 | motion::waypoint_flag_mutex.lock(); |
rsavitski | 69:4b7bb92916da | 55 | if (motion::checkMotionStatus() && (c_upper.getColour()==RED || firstavoidstop) && delayed_done) |
rsavitski | 30:791739422122 | 56 | { |
rsavitski | 65:4709ff6c753c | 57 | //temphack!!!!! |
rsavitski | 69:4b7bb92916da | 58 | //Thread::wait(1000); |
rsavitski | 65:4709ff6c753c | 59 | arm::upper_arm.go_down(); |
rsavitski | 69:4b7bb92916da | 60 | delayed_done = false; |
rsavitski | 70:0da6ca845762 | 61 | delayed_armer.start(1200); |
rsavitski | 70:0da6ca845762 | 62 | delayed_wp_set.start(2400); |
rsavitski | 69:4b7bb92916da | 63 | //Thread::wait(2000); |
rsavitski | 69:4b7bb92916da | 64 | //arm::upper_arm.go_up(); |
rsavitski | 65:4709ff6c753c | 65 | |
rsavitski | 54:99d3158c9207 | 66 | phi -= 22.5/180*PI; |
rsavitski | 54:99d3158c9207 | 67 | current_waypoint.x = 1.5-r*cos(phi); |
rsavitski | 54:99d3158c9207 | 68 | current_waypoint.y = 2-r*sin(phi); |
rsavitski | 54:99d3158c9207 | 69 | current_waypoint.theta = constrainAngle(phi+PI/2); |
rsavitski | 56:ed585a82092b | 70 | |
rsavitski | 56:ed585a82092b | 71 | //arm offset |
rsavitski | 56:ed585a82092b | 72 | current_waypoint.x += 0.0425*cos(current_waypoint.theta); |
rsavitski | 56:ed585a82092b | 73 | current_waypoint.y += 0.0425*sin(current_waypoint.theta); |
madcowswe | 67:be3ea5450cc7 | 74 | |
madcowswe | 67:be3ea5450cc7 | 75 | current_waypoint.pos_threshold = 0.01; |
madcowswe | 67:be3ea5450cc7 | 76 | current_waypoint.angle_threshold = 0.01*PI; |
rsavitski | 57:d434ceab6892 | 77 | |
rsavitski | 69:4b7bb92916da | 78 | //motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); |
rsavitski | 69:4b7bb92916da | 79 | if (firstavoidstop) |
rsavitski | 69:4b7bb92916da | 80 | { |
madcowswe | 66:f1d75e51398d | 81 | motion::collavoiden = 0; |
madcowswe | 66:f1d75e51398d | 82 | firstavoidstop = 0; |
madcowswe | 66:f1d75e51398d | 83 | } |
madcowswe | 66:f1d75e51398d | 84 | else |
madcowswe | 66:f1d75e51398d | 85 | motion::collavoiden = 1; |
rsavitski | 30:791739422122 | 86 | } |
rsavitski | 54:99d3158c9207 | 87 | motion::waypoint_flag_mutex.unlock(); |
rsavitski | 54:99d3158c9207 | 88 | |
rsavitski | 54:99d3158c9207 | 89 | Thread::wait(50); |
rsavitski | 30:791739422122 | 90 | } |
rsavitski | 30:791739422122 | 91 | } |
rsavitski | 30:791739422122 | 92 | |
rsavitski | 70:0da6ca845762 | 93 | void arm_upper(const void *dummy) |
rsavitski | 70:0da6ca845762 | 94 | { |
rsavitski | 70:0da6ca845762 | 95 | arm::upper_arm.go_up(); |
rsavitski | 70:0da6ca845762 | 96 | } |
rsavitski | 70:0da6ca845762 | 97 | |
rsavitski | 69:4b7bb92916da | 98 | void delayed_setter(const void *tid_wpptr) //TODO: kill the hack |
rsavitski | 69:4b7bb92916da | 99 | { |
rsavitski | 69:4b7bb92916da | 100 | delayed_struct *dsptr = (delayed_struct *)tid_wpptr; |
rsavitski | 70:0da6ca845762 | 101 | //arm::upper_arm.go_up(); |
rsavitski | 69:4b7bb92916da | 102 | motion::setNewWaypoint(dsptr->tid,dsptr->wpptr); |
rsavitski | 69:4b7bb92916da | 103 | delayed_done = true; |
rsavitski | 69:4b7bb92916da | 104 | } |
rsavitski | 69:4b7bb92916da | 105 | |
rsavitski | 30:791739422122 | 106 | } //namespace |