This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Processes/AI/ai.cpp@78:3178a1e46146, 2013-04-15 (annotated)
- Committer:
- rsavitski
- Date:
- Mon Apr 15 17:07:40 2013 +0000
- Revision:
- 78:3178a1e46146
- Parent:
- 74:9620d24a2f4e
- Parent:
- 77:8d83a0c00e66
- Child:
- 81:ef1ce4f5322b
merged x2
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 | 77:8d83a0c00e66 | 8 | #include "MotorControl.h" |
rsavitski | 55:0c8897da6b3a | 9 | |
rsavitski | 77:8d83a0c00e66 | 10 | //TODO: after 2012/2013, kill entire AI layer as it is hacked together. Rest is fine-ish |
rsavitski | 77:8d83a0c00e66 | 11 | |
rsavitski | 77:8d83a0c00e66 | 12 | Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER); |
rsavitski | 77:8d83a0c00e66 | 13 | Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER); |
rsavitski | 30:791739422122 | 14 | |
rsavitski | 30:791739422122 | 15 | namespace AI |
rsavitski | 30:791739422122 | 16 | { |
rsavitski | 77:8d83a0c00e66 | 17 | // starting position |
rsavitski | 77:8d83a0c00e66 | 18 | #ifdef TEAM_RED |
rsavitski | 77:8d83a0c00e66 | 19 | ColourEnum own_colour = RED; |
rsavitski | 77:8d83a0c00e66 | 20 | |
rsavitski | 78:3178a1e46146 | 21 | Waypoint home_wp = {2.7, 1.75, PI, 0.03, 0.05*PI, 32}; |
rsavitski | 77:8d83a0c00e66 | 22 | #endif |
rsavitski | 77:8d83a0c00e66 | 23 | |
rsavitski | 77:8d83a0c00e66 | 24 | #ifdef TEAM_BLUE |
rsavitski | 77:8d83a0c00e66 | 25 | ColourEnum own_colour = BLUE; |
rsavitski | 77:8d83a0c00e66 | 26 | |
rsavitski | 78:3178a1e46146 | 27 | Waypoint home_wp = {0.3, 1, 0, 0.03, 0.05*PI, 32}; |
rsavitski | 77:8d83a0c00e66 | 28 | #endif |
rsavitski | 30:791739422122 | 29 | |
rsavitski | 69:4b7bb92916da | 30 | bool delayed_done = true; //TODO: kill |
rsavitski | 69:4b7bb92916da | 31 | |
rsavitski | 69:4b7bb92916da | 32 | struct delayed_struct //TODO: kill |
rsavitski | 69:4b7bb92916da | 33 | { |
rsavitski | 69:4b7bb92916da | 34 | osThreadId tid; |
rsavitski | 69:4b7bb92916da | 35 | Waypoint *wpptr; |
rsavitski | 69:4b7bb92916da | 36 | }; |
rsavitski | 69:4b7bb92916da | 37 | |
rsavitski | 76:532d9bc1d2aa | 38 | void raise_top_arm(const void *dummy); |
rsavitski | 76:532d9bc1d2aa | 39 | void raise_bottom_arm(const void *dummy); |
rsavitski | 70:0da6ca845762 | 40 | |
rsavitski | 69:4b7bb92916da | 41 | void delayed_setter(const void *tid_wpptr); //TODO: kill the hack |
rsavitski | 69:4b7bb92916da | 42 | |
rsavitski | 30:791739422122 | 43 | void ailayer(void const *dummy) |
rsavitski | 30:791739422122 | 44 | { |
rsavitski | 78:3178a1e46146 | 45 | RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce); |
rsavitski | 78:3178a1e46146 | 46 | RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce); |
rsavitski | 78:3178a1e46146 | 47 | |
rsavitski | 78:3178a1e46146 | 48 | //NB: reassign ds.wpptr before each invocation |
rsavitski | 78:3178a1e46146 | 49 | delayed_struct ds = {Thread::gettid(),NULL}; |
rsavitski | 78:3178a1e46146 | 50 | RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds); |
rsavitski | 78:3178a1e46146 | 51 | |
rsavitski | 77:8d83a0c00e66 | 52 | MotorControl::motorsenabled = true; |
rsavitski | 77:8d83a0c00e66 | 53 | motion::collavoiden = 1; |
rsavitski | 77:8d83a0c00e66 | 54 | |
rsavitski | 77:8d83a0c00e66 | 55 | motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home |
rsavitski | 78:3178a1e46146 | 56 | Thread::signal_wait(0x1); //wait until wp reached |
rsavitski | 78:3178a1e46146 | 57 | |
rsavitski | 77:8d83a0c00e66 | 58 | MotorControl::motorsenabled = false; |
rsavitski | 77:8d83a0c00e66 | 59 | |
rsavitski | 78:3178a1e46146 | 60 | Thread::signal_wait(0x2); //wait for cord |
rsavitski | 77:8d83a0c00e66 | 61 | |
rsavitski | 77:8d83a0c00e66 | 62 | // CORD PULLED |
rsavitski | 77:8d83a0c00e66 | 63 | |
rsavitski | 78:3178a1e46146 | 64 | Waypoint mid_wp = {1.8, 1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32}; |
rsavitski | 77:8d83a0c00e66 | 65 | |
rsavitski | 77:8d83a0c00e66 | 66 | MotorControl::motorsenabled = true; |
rsavitski | 78:3178a1e46146 | 67 | motion::setNewWaypoint(Thread::gettid(),&mid_wp); |
rsavitski | 78:3178a1e46146 | 68 | Thread::signal_wait(0x1); //wait until wp reached |
rsavitski | 78:3178a1e46146 | 69 | |
rsavitski | 78:3178a1e46146 | 70 | Waypoint approach_wp = {2.2, 1.85, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32}; |
rsavitski | 78:3178a1e46146 | 71 | motion::setNewWaypoint(Thread::gettid(),&approach_wp); |
rsavitski | 78:3178a1e46146 | 72 | Thread::signal_wait(0x1); //wait until wp reached |
rsavitski | 77:8d83a0c00e66 | 73 | |
rsavitski | 78:3178a1e46146 | 74 | // TODO: ?(disable collison avoidance), approach first cake wp, enable collision |
rsavitski | 78:3178a1e46146 | 75 | /* |
rsavitski | 78:3178a1e46146 | 76 | ///////////////temp hack |
rsavitski | 78:3178a1e46146 | 77 | Waypoint temp_wp = {1.222,1.440, 149.0/180*PI, 0.01, 0.02*PI, 512}; |
rsavitski | 78:3178a1e46146 | 78 | motion::setNewWaypoint(Thread::gettid(),&temp_wp); |
rsavitski | 78:3178a1e46146 | 79 | Thread::signal_wait(0x1); //wait until wp reached |
rsavitski | 78:3178a1e46146 | 80 | arm::lower_arm.go_down(); |
rsavitski | 78:3178a1e46146 | 81 | bottom_arm_up_timer.start(1200); |
rsavitski | 78:3178a1e46146 | 82 | /////////////// temp hack over |
rsavitski | 78:3178a1e46146 | 83 | */ |
rsavitski | 77:8d83a0c00e66 | 84 | |
rsavitski | 78:3178a1e46146 | 85 | while(1) |
rsavitski | 78:3178a1e46146 | 86 | Thread::wait(1000); |
rsavitski | 78:3178a1e46146 | 87 | /* |
rsavitski | 77:8d83a0c00e66 | 88 | /////////////////////////////////////////////////////// |
rsavitski | 55:0c8897da6b3a | 89 | |
rsavitski | 69:4b7bb92916da | 90 | // first waypoint for approach |
rsavitski | 54:99d3158c9207 | 91 | current_waypoint.x = 2.2; |
rsavitski | 54:99d3158c9207 | 92 | current_waypoint.y = 1.85; |
madcowswe | 67:be3ea5450cc7 | 93 | current_waypoint.theta = (-3.0f/4.0f)*PI; |
madcowswe | 67:be3ea5450cc7 | 94 | current_waypoint.pos_threshold = 0.03; |
rsavitski | 54:99d3158c9207 | 95 | current_waypoint.angle_threshold = 0.02*PI; |
rsavitski | 69:4b7bb92916da | 96 | |
madcowswe | 66:f1d75e51398d | 97 | motion::collavoiden = 1; |
madcowswe | 74:9620d24a2f4e | 98 | MotorControl::motorsenabled = 1; |
rsavitski | 69:4b7bb92916da | 99 | motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); |
rsavitski | 55:0c8897da6b3a | 100 | |
rsavitski | 78:3178a1e46146 | 101 | float r = 0.26+0.35+0.02+0.01; |
rsavitski | 53:b013df99b747 | 102 | |
madcowswe | 66:f1d75e51398d | 103 | bool firstavoidstop = 1; |
rsavitski | 69:4b7bb92916da | 104 | delayed_struct ds = {Thread::gettid(),¤t_waypoint}; |
rsavitski | 70:0da6ca845762 | 105 | RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds); |
rsavitski | 76:532d9bc1d2aa | 106 | RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce); |
rsavitski | 69:4b7bb92916da | 107 | |
rsavitski | 54:99d3158c9207 | 108 | for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;) |
rsavitski | 30:791739422122 | 109 | { |
rsavitski | 39:44d3dea4adcc | 110 | motion::waypoint_flag_mutex.lock(); |
rsavitski | 77:8d83a0c00e66 | 111 | if (motion::checkMotionStatus() && (c_upper.getColour()==own_colour || firstavoidstop) && delayed_done) |
rsavitski | 30:791739422122 | 112 | { |
rsavitski | 65:4709ff6c753c | 113 | //temphack!!!!! |
rsavitski | 69:4b7bb92916da | 114 | //Thread::wait(1000); |
rsavitski | 65:4709ff6c753c | 115 | arm::upper_arm.go_down(); |
rsavitski | 69:4b7bb92916da | 116 | delayed_done = false; |
rsavitski | 76:532d9bc1d2aa | 117 | top_arm_up_timer.start(1200); |
rsavitski | 70:0da6ca845762 | 118 | delayed_wp_set.start(2400); |
rsavitski | 69:4b7bb92916da | 119 | //Thread::wait(2000); |
rsavitski | 69:4b7bb92916da | 120 | //arm::upper_arm.go_up(); |
rsavitski | 65:4709ff6c753c | 121 | |
rsavitski | 54:99d3158c9207 | 122 | phi -= 22.5/180*PI; |
rsavitski | 54:99d3158c9207 | 123 | current_waypoint.x = 1.5-r*cos(phi); |
rsavitski | 54:99d3158c9207 | 124 | current_waypoint.y = 2-r*sin(phi); |
rsavitski | 54:99d3158c9207 | 125 | current_waypoint.theta = constrainAngle(phi+PI/2); |
rsavitski | 56:ed585a82092b | 126 | |
rsavitski | 56:ed585a82092b | 127 | //arm offset |
rsavitski | 56:ed585a82092b | 128 | current_waypoint.x += 0.0425*cos(current_waypoint.theta); |
rsavitski | 56:ed585a82092b | 129 | current_waypoint.y += 0.0425*sin(current_waypoint.theta); |
madcowswe | 67:be3ea5450cc7 | 130 | |
madcowswe | 67:be3ea5450cc7 | 131 | current_waypoint.pos_threshold = 0.01; |
madcowswe | 67:be3ea5450cc7 | 132 | current_waypoint.angle_threshold = 0.01*PI; |
rsavitski | 57:d434ceab6892 | 133 | |
rsavitski | 69:4b7bb92916da | 134 | //motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); |
rsavitski | 69:4b7bb92916da | 135 | if (firstavoidstop) |
rsavitski | 69:4b7bb92916da | 136 | { |
madcowswe | 66:f1d75e51398d | 137 | motion::collavoiden = 0; |
madcowswe | 66:f1d75e51398d | 138 | firstavoidstop = 0; |
madcowswe | 66:f1d75e51398d | 139 | } |
madcowswe | 66:f1d75e51398d | 140 | else |
madcowswe | 66:f1d75e51398d | 141 | motion::collavoiden = 1; |
rsavitski | 30:791739422122 | 142 | } |
rsavitski | 54:99d3158c9207 | 143 | motion::waypoint_flag_mutex.unlock(); |
rsavitski | 54:99d3158c9207 | 144 | |
rsavitski | 54:99d3158c9207 | 145 | Thread::wait(50); |
rsavitski | 78:3178a1e46146 | 146 | }*/ |
rsavitski | 30:791739422122 | 147 | } |
rsavitski | 30:791739422122 | 148 | |
rsavitski | 76:532d9bc1d2aa | 149 | void raise_top_arm(const void *dummy) |
rsavitski | 70:0da6ca845762 | 150 | { |
rsavitski | 70:0da6ca845762 | 151 | arm::upper_arm.go_up(); |
rsavitski | 70:0da6ca845762 | 152 | } |
rsavitski | 70:0da6ca845762 | 153 | |
rsavitski | 76:532d9bc1d2aa | 154 | void raise_bottom_arm(const void *dummy) |
rsavitski | 76:532d9bc1d2aa | 155 | { |
rsavitski | 76:532d9bc1d2aa | 156 | arm::lower_arm.go_up(); |
rsavitski | 76:532d9bc1d2aa | 157 | } |
rsavitski | 76:532d9bc1d2aa | 158 | |
rsavitski | 69:4b7bb92916da | 159 | void delayed_setter(const void *tid_wpptr) //TODO: kill the hack |
rsavitski | 69:4b7bb92916da | 160 | { |
rsavitski | 69:4b7bb92916da | 161 | delayed_struct *dsptr = (delayed_struct *)tid_wpptr; |
rsavitski | 69:4b7bb92916da | 162 | motion::setNewWaypoint(dsptr->tid,dsptr->wpptr); |
rsavitski | 69:4b7bb92916da | 163 | delayed_done = true; |
rsavitski | 69:4b7bb92916da | 164 | } |
rsavitski | 69:4b7bb92916da | 165 | |
rsavitski | 30:791739422122 | 166 | } //namespace |