![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Processes/AI/ai.cpp@90:e4164bb8c60e, 2013-04-17 (annotated)
- Committer:
- madcowswe
- Date:
- Wed Apr 17 23:16:25 2013 +0000
- Revision:
- 90:e4164bb8c60e
- Parent:
- 89:dfe8c2ec5b88
final state at end of competition. Includes avoid wooden team hack
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 | 83:223186cd7531 | 12 | //#define HARDCODED_WAYPOINTS_HACK |
rsavitski | 83:223186cd7531 | 13 | |
rsavitski | 77:8d83a0c00e66 | 14 | Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER); |
rsavitski | 77:8d83a0c00e66 | 15 | Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER); |
rsavitski | 30:791739422122 | 16 | |
rsavitski | 30:791739422122 | 17 | namespace AI |
rsavitski | 30:791739422122 | 18 | { |
rsavitski | 77:8d83a0c00e66 | 19 | // starting position |
rsavitski | 77:8d83a0c00e66 | 20 | #ifdef TEAM_RED |
rsavitski | 77:8d83a0c00e66 | 21 | ColourEnum own_colour = RED; |
rsavitski | 77:8d83a0c00e66 | 22 | |
rsavitski | 78:3178a1e46146 | 23 | Waypoint home_wp = {2.7, 1.75, PI, 0.03, 0.05*PI, 32}; |
rsavitski | 77:8d83a0c00e66 | 24 | #endif |
rsavitski | 77:8d83a0c00e66 | 25 | |
rsavitski | 77:8d83a0c00e66 | 26 | #ifdef TEAM_BLUE |
rsavitski | 77:8d83a0c00e66 | 27 | ColourEnum own_colour = BLUE; |
rsavitski | 77:8d83a0c00e66 | 28 | |
rsavitski | 78:3178a1e46146 | 29 | Waypoint home_wp = {0.3, 1, 0, 0.03, 0.05*PI, 32}; |
rsavitski | 77:8d83a0c00e66 | 30 | #endif |
rsavitski | 30:791739422122 | 31 | |
rsavitski | 81:ef1ce4f5322b | 32 | enum layer { top_layer, bot_layer }; |
rsavitski | 81:ef1ce4f5322b | 33 | |
rsavitski | 69:4b7bb92916da | 34 | bool delayed_done = true; //TODO: kill |
rsavitski | 69:4b7bb92916da | 35 | |
rsavitski | 69:4b7bb92916da | 36 | struct delayed_struct //TODO: kill |
rsavitski | 69:4b7bb92916da | 37 | { |
rsavitski | 69:4b7bb92916da | 38 | osThreadId tid; |
rsavitski | 69:4b7bb92916da | 39 | Waypoint *wpptr; |
rsavitski | 69:4b7bb92916da | 40 | }; |
rsavitski | 69:4b7bb92916da | 41 | |
rsavitski | 76:532d9bc1d2aa | 42 | void raise_top_arm(const void *dummy); |
rsavitski | 76:532d9bc1d2aa | 43 | void raise_bottom_arm(const void *dummy); |
rsavitski | 70:0da6ca845762 | 44 | |
rsavitski | 69:4b7bb92916da | 45 | void delayed_setter(const void *tid_wpptr); //TODO: kill the hack |
rsavitski | 69:4b7bb92916da | 46 | |
rsavitski | 83:223186cd7531 | 47 | #ifndef HARDCODED_WAYPOINTS_HACK |
rsavitski | 30:791739422122 | 48 | void ailayer(void const *dummy) |
rsavitski | 30:791739422122 | 49 | { |
rsavitski | 78:3178a1e46146 | 50 | RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce); |
rsavitski | 78:3178a1e46146 | 51 | RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce); |
rsavitski | 78:3178a1e46146 | 52 | |
rsavitski | 78:3178a1e46146 | 53 | //NB: reassign ds.wpptr before each invocation |
rsavitski | 83:223186cd7531 | 54 | //delayed_struct ds = {Thread::gettid(),NULL}; |
rsavitski | 83:223186cd7531 | 55 | //RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds); |
rsavitski | 78:3178a1e46146 | 56 | |
rsavitski | 77:8d83a0c00e66 | 57 | MotorControl::motorsenabled = true; |
rsavitski | 77:8d83a0c00e66 | 58 | motion::collavoiden = 1; |
rsavitski | 77:8d83a0c00e66 | 59 | |
rsavitski | 81:ef1ce4f5322b | 60 | motion::waypoint_flag_mutex.lock(); |
rsavitski | 77:8d83a0c00e66 | 61 | motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home |
rsavitski | 81:ef1ce4f5322b | 62 | motion::waypoint_flag_mutex.unlock(); |
rsavitski | 78:3178a1e46146 | 63 | Thread::signal_wait(0x1); //wait until wp reached |
rsavitski | 78:3178a1e46146 | 64 | |
rsavitski | 77:8d83a0c00e66 | 65 | MotorControl::motorsenabled = false; |
rsavitski | 81:ef1ce4f5322b | 66 | arm::upper_arm.go_up(); |
madcowswe | 85:b0858346d838 | 67 | arm::lower_arm.go_down(); |
rsavitski | 77:8d83a0c00e66 | 68 | |
rsavitski | 78:3178a1e46146 | 69 | Thread::signal_wait(0x2); //wait for cord |
rsavitski | 77:8d83a0c00e66 | 70 | |
rsavitski | 77:8d83a0c00e66 | 71 | // CORD PULLED |
rsavitski | 81:ef1ce4f5322b | 72 | MotorControl::motorsenabled = true; |
madcowswe | 85:b0858346d838 | 73 | arm::lower_arm.go_up(); |
rsavitski | 77:8d83a0c00e66 | 74 | |
rsavitski | 81:ef1ce4f5322b | 75 | #ifdef TEAM_BLUE |
madcowswe | 90:e4164bb8c60e | 76 | |
madcowswe | 90:e4164bb8c60e | 77 | Waypoint mid_wp5 = {1.2, 1, 0, 0.10, 0.15*PI, 32}; |
madcowswe | 90:e4164bb8c60e | 78 | motion::waypoint_flag_mutex.lock(); |
madcowswe | 90:e4164bb8c60e | 79 | motion::setNewWaypoint(Thread::gettid(),&mid_wp5); |
madcowswe | 90:e4164bb8c60e | 80 | motion::waypoint_flag_mutex.unlock(); |
madcowswe | 90:e4164bb8c60e | 81 | Thread::signal_wait(0x1); //wait until wp reached |
madcowswe | 90:e4164bb8c60e | 82 | Thread::wait(3000); |
madcowswe | 90:e4164bb8c60e | 83 | |
madcowswe | 90:e4164bb8c60e | 84 | Waypoint mid_wp = {1.9, 1.1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32}; |
rsavitski | 81:ef1ce4f5322b | 85 | motion::waypoint_flag_mutex.lock(); |
rsavitski | 78:3178a1e46146 | 86 | motion::setNewWaypoint(Thread::gettid(),&mid_wp); |
rsavitski | 81:ef1ce4f5322b | 87 | motion::waypoint_flag_mutex.unlock(); |
rsavitski | 78:3178a1e46146 | 88 | Thread::signal_wait(0x1); //wait until wp reached |
rsavitski | 78:3178a1e46146 | 89 | |
madcowswe | 90:e4164bb8c60e | 90 | Waypoint approach_wp = {2.24, 1.84/*1.85*/, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32}; |
rsavitski | 81:ef1ce4f5322b | 91 | motion::waypoint_flag_mutex.lock(); |
rsavitski | 78:3178a1e46146 | 92 | motion::setNewWaypoint(Thread::gettid(),&approach_wp); |
rsavitski | 81:ef1ce4f5322b | 93 | motion::waypoint_flag_mutex.unlock(); |
rsavitski | 78:3178a1e46146 | 94 | Thread::signal_wait(0x1); //wait until wp reached |
madcowswe | 88:8850373c3f0d | 95 | #endif |
madcowswe | 88:8850373c3f0d | 96 | /* |
madcowswe | 88:8850373c3f0d | 97 | Waypoint approach_wp2 = {2.2-0.05, 1.83, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32}; |
madcowswe | 88:8850373c3f0d | 98 | motion::waypoint_flag_mutex.lock(); |
madcowswe | 88:8850373c3f0d | 99 | motion::setNewWaypoint(Thread::gettid(),&approach_wp2); |
madcowswe | 88:8850373c3f0d | 100 | motion::waypoint_flag_mutex.unlock(); |
madcowswe | 88:8850373c3f0d | 101 | Thread::signal_wait(0x1); //wait until wp reached*/ |
rsavitski | 77:8d83a0c00e66 | 102 | |
rsavitski | 81:ef1ce4f5322b | 103 | Waypoint mutable_cake_wp = {0, 0, 0, 0.01, 0.01*PI, 512}; |
rsavitski | 81:ef1ce4f5322b | 104 | |
madcowswe | 86:769e33a3f0ff | 105 | float r = 0.26+0.35-0.0075;//-0.01; //second 0.01 for being less collisiony with sensors |
rsavitski | 81:ef1ce4f5322b | 106 | |
rsavitski | 81:ef1ce4f5322b | 107 | layer layer_to_push = top_layer; |
rsavitski | 81:ef1ce4f5322b | 108 | |
madcowswe | 88:8850373c3f0d | 109 | float top_target_phi = (180-(11.25/*+22.5*/))/180*PI; |
rsavitski | 81:ef1ce4f5322b | 110 | float bot_target_phi = (180-(7.5+15))/180*PI; |
rsavitski | 81:ef1ce4f5322b | 111 | |
rsavitski | 81:ef1ce4f5322b | 112 | float phi = 0; // angle of vector (robot->center of cake) |
rsavitski | 81:ef1ce4f5322b | 113 | |
madcowswe | 88:8850373c3f0d | 114 | for(int i=0; i<18; ++i) |
rsavitski | 81:ef1ce4f5322b | 115 | { |
rsavitski | 81:ef1ce4f5322b | 116 | if (top_target_phi > bot_target_phi) |
rsavitski | 81:ef1ce4f5322b | 117 | { |
rsavitski | 81:ef1ce4f5322b | 118 | layer_to_push = top_layer; |
rsavitski | 81:ef1ce4f5322b | 119 | phi = top_target_phi; |
rsavitski | 81:ef1ce4f5322b | 120 | top_target_phi -= 22.5/180*PI; |
rsavitski | 81:ef1ce4f5322b | 121 | } |
rsavitski | 81:ef1ce4f5322b | 122 | else |
rsavitski | 81:ef1ce4f5322b | 123 | { |
rsavitski | 81:ef1ce4f5322b | 124 | layer_to_push = bot_layer; |
rsavitski | 81:ef1ce4f5322b | 125 | phi = bot_target_phi; |
rsavitski | 81:ef1ce4f5322b | 126 | bot_target_phi -= 15.0/180*PI; |
rsavitski | 81:ef1ce4f5322b | 127 | } |
rsavitski | 81:ef1ce4f5322b | 128 | |
madcowswe | 89:dfe8c2ec5b88 | 129 | // hack |
madcowswe | 89:dfe8c2ec5b88 | 130 | if (own_colour==BLUE && i==0) |
madcowswe | 89:dfe8c2ec5b88 | 131 | continue; |
madcowswe | 89:dfe8c2ec5b88 | 132 | |
rsavitski | 81:ef1ce4f5322b | 133 | // set new wp |
rsavitski | 81:ef1ce4f5322b | 134 | mutable_cake_wp.x = 1.5-r*cos(phi); |
rsavitski | 81:ef1ce4f5322b | 135 | mutable_cake_wp.y = 2-r*sin(phi); |
rsavitski | 81:ef1ce4f5322b | 136 | mutable_cake_wp.theta = constrainAngle(phi+PI/2); |
rsavitski | 81:ef1ce4f5322b | 137 | |
rsavitski | 81:ef1ce4f5322b | 138 | //arm offset |
rsavitski | 81:ef1ce4f5322b | 139 | mutable_cake_wp.x += 0.0425*cos(mutable_cake_wp.theta); |
rsavitski | 81:ef1ce4f5322b | 140 | mutable_cake_wp.y += 0.0425*sin(mutable_cake_wp.theta); |
rsavitski | 81:ef1ce4f5322b | 141 | |
rsavitski | 81:ef1ce4f5322b | 142 | |
rsavitski | 81:ef1ce4f5322b | 143 | motion::waypoint_flag_mutex.lock(); |
rsavitski | 81:ef1ce4f5322b | 144 | motion::setNewWaypoint(Thread::gettid(),&mutable_cake_wp); |
rsavitski | 81:ef1ce4f5322b | 145 | motion::waypoint_flag_mutex.unlock(); |
rsavitski | 81:ef1ce4f5322b | 146 | Thread::signal_wait(0x1); //wait until wp reached |
rsavitski | 81:ef1ce4f5322b | 147 | |
rsavitski | 81:ef1ce4f5322b | 148 | if(layer_to_push == top_layer) |
rsavitski | 81:ef1ce4f5322b | 149 | { |
rsavitski | 81:ef1ce4f5322b | 150 | ColourEnum colour_read = c_upper.getColour(); |
madcowswe | 88:8850373c3f0d | 151 | if ((colour_read==own_colour || i==0) && MotorControl::motorsenabled) |
rsavitski | 81:ef1ce4f5322b | 152 | { |
rsavitski | 81:ef1ce4f5322b | 153 | arm::upper_arm.go_down(); |
madcowswe | 88:8850373c3f0d | 154 | top_arm_up_timer.start(1200); |
rsavitski | 81:ef1ce4f5322b | 155 | } |
madcowswe | 88:8850373c3f0d | 156 | } |
rsavitski | 81:ef1ce4f5322b | 157 | else |
rsavitski | 81:ef1ce4f5322b | 158 | { |
rsavitski | 81:ef1ce4f5322b | 159 | ColourEnum colour_read = c_lower.getColour(); |
madcowswe | 88:8850373c3f0d | 160 | if ((colour_read==own_colour || i==5+1 || i==7+1 || i==8+1 || i==10+1/*|| colour_read==WHITE*/) && MotorControl::motorsenabled) |
rsavitski | 81:ef1ce4f5322b | 161 | { |
rsavitski | 81:ef1ce4f5322b | 162 | arm::lower_arm.go_down(); |
madcowswe | 88:8850373c3f0d | 163 | bottom_arm_up_timer.start(1200); |
rsavitski | 81:ef1ce4f5322b | 164 | } |
rsavitski | 81:ef1ce4f5322b | 165 | } |
madcowswe | 88:8850373c3f0d | 166 | Thread::wait(2100); |
rsavitski | 81:ef1ce4f5322b | 167 | } |
rsavitski | 81:ef1ce4f5322b | 168 | |
rsavitski | 81:ef1ce4f5322b | 169 | //////////////////// |
rsavitski | 77:8d83a0c00e66 | 170 | |
rsavitski | 78:3178a1e46146 | 171 | while(1) |
rsavitski | 78:3178a1e46146 | 172 | Thread::wait(1000); |
rsavitski | 30:791739422122 | 173 | } |
rsavitski | 83:223186cd7531 | 174 | #else |
rsavitski | 83:223186cd7531 | 175 | enum action_t {top_push_colour, bot_push_colour, bot_push_white}; |
rsavitski | 83:223186cd7531 | 176 | |
rsavitski | 83:223186cd7531 | 177 | void ailayer(void const *dummy) |
rsavitski | 83:223186cd7531 | 178 | { |
rsavitski | 83:223186cd7531 | 179 | RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce); |
rsavitski | 83:223186cd7531 | 180 | RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce); |
rsavitski | 83:223186cd7531 | 181 | |
rsavitski | 83:223186cd7531 | 182 | //////////////////////////////////// |
rsavitski | 83:223186cd7531 | 183 | // put waypoints here |
rsavitski | 83:223186cd7531 | 184 | //////////////////////////////////// |
rsavitski | 83:223186cd7531 | 185 | const int wp_num = 3; |
rsavitski | 83:223186cd7531 | 186 | |
rsavitski | 83:223186cd7531 | 187 | float x_arr[wp_num] = {1.2,1.5,1.7}; //<-------------------------- |
rsavitski | 83:223186cd7531 | 188 | float y_arr[wp_num] = {1,1.2,1.4}; //<-------------------------- |
rsavitski | 83:223186cd7531 | 189 | float theta_arr[wp_num] = {0,PI, PI/2}; //<---------------------- |
rsavitski | 83:223186cd7531 | 190 | action_t action[wp_num] = {top_push_colour, bot_push_colour, bot_push_white}; //<---------------------------- |
rsavitski | 83:223186cd7531 | 191 | |
rsavitski | 83:223186cd7531 | 192 | Waypoint wp_arr[wp_num]; |
rsavitski | 83:223186cd7531 | 193 | |
rsavitski | 83:223186cd7531 | 194 | for(int i=0; i<wp_num; ++i) |
rsavitski | 83:223186cd7531 | 195 | { |
rsavitski | 83:223186cd7531 | 196 | wp_arr[i].x =x_arr[i]; |
rsavitski | 83:223186cd7531 | 197 | wp_arr[i].y =y_arr[i]; |
rsavitski | 83:223186cd7531 | 198 | wp_arr[i].theta =theta_arr[i]; |
rsavitski | 83:223186cd7531 | 199 | |
rsavitski | 83:223186cd7531 | 200 | wp_arr[i].pos_threshold = 0.01; |
rsavitski | 83:223186cd7531 | 201 | wp_arr[i].angle_threshold = 0.01*PI; |
rsavitski | 83:223186cd7531 | 202 | wp_arr[i].angle_exponent = 512; |
rsavitski | 83:223186cd7531 | 203 | } |
rsavitski | 83:223186cd7531 | 204 | |
rsavitski | 83:223186cd7531 | 205 | //////////////////////////////////// |
rsavitski | 83:223186cd7531 | 206 | |
rsavitski | 83:223186cd7531 | 207 | MotorControl::motorsenabled = true; |
rsavitski | 83:223186cd7531 | 208 | motion::collavoiden = 1; |
rsavitski | 83:223186cd7531 | 209 | |
rsavitski | 83:223186cd7531 | 210 | motion::waypoint_flag_mutex.lock(); |
rsavitski | 83:223186cd7531 | 211 | motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home |
rsavitski | 83:223186cd7531 | 212 | motion::waypoint_flag_mutex.unlock(); |
rsavitski | 83:223186cd7531 | 213 | Thread::signal_wait(0x1); //wait until wp reached |
rsavitski | 83:223186cd7531 | 214 | |
rsavitski | 83:223186cd7531 | 215 | MotorControl::motorsenabled = false; |
rsavitski | 83:223186cd7531 | 216 | arm::upper_arm.go_up(); |
rsavitski | 83:223186cd7531 | 217 | arm::lower_arm.go_up(); |
rsavitski | 83:223186cd7531 | 218 | |
rsavitski | 83:223186cd7531 | 219 | Thread::signal_wait(0x2); //wait for cord |
rsavitski | 83:223186cd7531 | 220 | |
rsavitski | 83:223186cd7531 | 221 | // CORD PULLED |
rsavitski | 83:223186cd7531 | 222 | MotorControl::motorsenabled = true; |
rsavitski | 83:223186cd7531 | 223 | |
rsavitski | 83:223186cd7531 | 224 | #ifdef TEAM_BLUE |
rsavitski | 83:223186cd7531 | 225 | Waypoint mid_wp = {1.8, 1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32}; |
rsavitski | 83:223186cd7531 | 226 | motion::waypoint_flag_mutex.lock(); |
rsavitski | 83:223186cd7531 | 227 | motion::setNewWaypoint(Thread::gettid(),&mid_wp); |
rsavitski | 83:223186cd7531 | 228 | motion::waypoint_flag_mutex.unlock(); |
rsavitski | 83:223186cd7531 | 229 | Thread::signal_wait(0x1); //wait until wp reached |
rsavitski | 83:223186cd7531 | 230 | #endif |
rsavitski | 83:223186cd7531 | 231 | |
rsavitski | 83:223186cd7531 | 232 | Waypoint approach_wp = {2.2, 1.85, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32}; |
rsavitski | 83:223186cd7531 | 233 | motion::waypoint_flag_mutex.lock(); |
rsavitski | 83:223186cd7531 | 234 | motion::setNewWaypoint(Thread::gettid(),&approach_wp); |
rsavitski | 83:223186cd7531 | 235 | motion::waypoint_flag_mutex.unlock(); |
rsavitski | 83:223186cd7531 | 236 | Thread::signal_wait(0x1); //wait until wp reached |
rsavitski | 83:223186cd7531 | 237 | |
rsavitski | 83:223186cd7531 | 238 | |
rsavitski | 83:223186cd7531 | 239 | for(int i=0; i<wp_num; ++i) |
rsavitski | 83:223186cd7531 | 240 | { |
rsavitski | 83:223186cd7531 | 241 | motion::waypoint_flag_mutex.lock(); |
rsavitski | 83:223186cd7531 | 242 | motion::setNewWaypoint(Thread::gettid(),&(wp_arr[i])); //go to home |
rsavitski | 83:223186cd7531 | 243 | motion::waypoint_flag_mutex.unlock(); |
rsavitski | 83:223186cd7531 | 244 | Thread::signal_wait(0x1); //wait until wp reached |
rsavitski | 83:223186cd7531 | 245 | |
rsavitski | 83:223186cd7531 | 246 | switch(action[i]) |
rsavitski | 83:223186cd7531 | 247 | { |
rsavitski | 83:223186cd7531 | 248 | case top_push_colour: |
madcowswe | 84:00c799fd10a7 | 249 | if ((c_upper.getColour()==own_colour) && MotorControl::motorsenabled) |
rsavitski | 83:223186cd7531 | 250 | { |
rsavitski | 83:223186cd7531 | 251 | arm::upper_arm.go_down(); |
rsavitski | 83:223186cd7531 | 252 | top_arm_up_timer.start(1200); |
rsavitski | 83:223186cd7531 | 253 | } |
rsavitski | 83:223186cd7531 | 254 | break; |
rsavitski | 83:223186cd7531 | 255 | case bot_push_colour: |
madcowswe | 84:00c799fd10a7 | 256 | if ((c_lower.getColour()==own_colour) && MotorControl::motorsenabled) |
rsavitski | 83:223186cd7531 | 257 | { |
rsavitski | 83:223186cd7531 | 258 | arm::lower_arm.go_down(); |
rsavitski | 83:223186cd7531 | 259 | bottom_arm_up_timer.start(1200); |
rsavitski | 83:223186cd7531 | 260 | } |
rsavitski | 83:223186cd7531 | 261 | break; |
rsavitski | 83:223186cd7531 | 262 | case bot_push_white: |
madcowswe | 84:00c799fd10a7 | 263 | if (MotorControl::motorsenabled) |
madcowswe | 84:00c799fd10a7 | 264 | { |
madcowswe | 84:00c799fd10a7 | 265 | arm::lower_arm.go_down(); |
madcowswe | 84:00c799fd10a7 | 266 | bottom_arm_up_timer.start(1200); |
madcowswe | 84:00c799fd10a7 | 267 | } |
rsavitski | 83:223186cd7531 | 268 | break; |
rsavitski | 83:223186cd7531 | 269 | } |
rsavitski | 83:223186cd7531 | 270 | |
rsavitski | 83:223186cd7531 | 271 | Thread::wait(2200); |
rsavitski | 83:223186cd7531 | 272 | } |
rsavitski | 83:223186cd7531 | 273 | |
rsavitski | 83:223186cd7531 | 274 | while(1) |
rsavitski | 83:223186cd7531 | 275 | Thread::wait(1000); |
rsavitski | 83:223186cd7531 | 276 | } |
rsavitski | 83:223186cd7531 | 277 | #endif |
rsavitski | 30:791739422122 | 278 | |
rsavitski | 76:532d9bc1d2aa | 279 | void raise_top_arm(const void *dummy) |
rsavitski | 70:0da6ca845762 | 280 | { |
rsavitski | 70:0da6ca845762 | 281 | arm::upper_arm.go_up(); |
rsavitski | 70:0da6ca845762 | 282 | } |
rsavitski | 70:0da6ca845762 | 283 | |
rsavitski | 76:532d9bc1d2aa | 284 | void raise_bottom_arm(const void *dummy) |
rsavitski | 76:532d9bc1d2aa | 285 | { |
rsavitski | 76:532d9bc1d2aa | 286 | arm::lower_arm.go_up(); |
rsavitski | 76:532d9bc1d2aa | 287 | } |
rsavitski | 76:532d9bc1d2aa | 288 | |
rsavitski | 69:4b7bb92916da | 289 | void delayed_setter(const void *tid_wpptr) //TODO: kill the hack |
rsavitski | 69:4b7bb92916da | 290 | { |
rsavitski | 69:4b7bb92916da | 291 | delayed_struct *dsptr = (delayed_struct *)tid_wpptr; |
rsavitski | 69:4b7bb92916da | 292 | motion::setNewWaypoint(dsptr->tid,dsptr->wpptr); |
rsavitski | 69:4b7bb92916da | 293 | delayed_done = true; |
rsavitski | 69:4b7bb92916da | 294 | } |
rsavitski | 69:4b7bb92916da | 295 | |
rsavitski | 30:791739422122 | 296 | } //namespace |