Oskar Weigl / Mbed 2 deprecated ICRSEurobot13

This is some awesome robot code

Dependencies:   mbed-rtos mbed QEI

Fork of ICRSEurobot13 by Thomas Branch

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?

UserRevisionLine numberNew 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