Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/AI/ai.cpp@89:dfe8c2ec5b88, 2013-04-16 (annotated)

Committer:
madcowswe
Date:
Tue Apr 16 13:19:17 2013 +0000
Revision:
89:dfe8c2ec5b88
Parent:
88:8850373c3f0d
Child:
90:e4164bb8c60e
blue works

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
rsavitski 81:ef1ce4f5322b 76 Waypoint mid_wp = {1.8, 1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
rsavitski 81:ef1ce4f5322b 77 motion::waypoint_flag_mutex.lock();
rsavitski 78:3178a1e46146 78 motion::setNewWaypoint(Thread::gettid(),&mid_wp);
rsavitski 81:ef1ce4f5322b 79 motion::waypoint_flag_mutex.unlock();
rsavitski 78:3178a1e46146 80 Thread::signal_wait(0x1); //wait until wp reached
rsavitski 78:3178a1e46146 81
madcowswe 89:dfe8c2ec5b88 82 Waypoint approach_wp = {2.24, 1.83/*1.85*/, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
rsavitski 81:ef1ce4f5322b 83 motion::waypoint_flag_mutex.lock();
rsavitski 78:3178a1e46146 84 motion::setNewWaypoint(Thread::gettid(),&approach_wp);
rsavitski 81:ef1ce4f5322b 85 motion::waypoint_flag_mutex.unlock();
rsavitski 78:3178a1e46146 86 Thread::signal_wait(0x1); //wait until wp reached
madcowswe 88:8850373c3f0d 87 #endif
madcowswe 88:8850373c3f0d 88 /*
madcowswe 88:8850373c3f0d 89 Waypoint approach_wp2 = {2.2-0.05, 1.83, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
madcowswe 88:8850373c3f0d 90 motion::waypoint_flag_mutex.lock();
madcowswe 88:8850373c3f0d 91 motion::setNewWaypoint(Thread::gettid(),&approach_wp2);
madcowswe 88:8850373c3f0d 92 motion::waypoint_flag_mutex.unlock();
madcowswe 88:8850373c3f0d 93 Thread::signal_wait(0x1); //wait until wp reached*/
rsavitski 77:8d83a0c00e66 94
rsavitski 81:ef1ce4f5322b 95 Waypoint mutable_cake_wp = {0, 0, 0, 0.01, 0.01*PI, 512};
rsavitski 81:ef1ce4f5322b 96
madcowswe 86:769e33a3f0ff 97 float r = 0.26+0.35-0.0075;//-0.01; //second 0.01 for being less collisiony with sensors
rsavitski 81:ef1ce4f5322b 98
rsavitski 81:ef1ce4f5322b 99 layer layer_to_push = top_layer;
rsavitski 81:ef1ce4f5322b 100
madcowswe 88:8850373c3f0d 101 float top_target_phi = (180-(11.25/*+22.5*/))/180*PI;
rsavitski 81:ef1ce4f5322b 102 float bot_target_phi = (180-(7.5+15))/180*PI;
rsavitski 81:ef1ce4f5322b 103
rsavitski 81:ef1ce4f5322b 104 float phi = 0; // angle of vector (robot->center of cake)
rsavitski 81:ef1ce4f5322b 105
madcowswe 88:8850373c3f0d 106 for(int i=0; i<18; ++i)
rsavitski 81:ef1ce4f5322b 107 {
rsavitski 81:ef1ce4f5322b 108 if (top_target_phi > bot_target_phi)
rsavitski 81:ef1ce4f5322b 109 {
rsavitski 81:ef1ce4f5322b 110 layer_to_push = top_layer;
rsavitski 81:ef1ce4f5322b 111 phi = top_target_phi;
rsavitski 81:ef1ce4f5322b 112 top_target_phi -= 22.5/180*PI;
rsavitski 81:ef1ce4f5322b 113 }
rsavitski 81:ef1ce4f5322b 114 else
rsavitski 81:ef1ce4f5322b 115 {
rsavitski 81:ef1ce4f5322b 116 layer_to_push = bot_layer;
rsavitski 81:ef1ce4f5322b 117 phi = bot_target_phi;
rsavitski 81:ef1ce4f5322b 118 bot_target_phi -= 15.0/180*PI;
rsavitski 81:ef1ce4f5322b 119 }
rsavitski 81:ef1ce4f5322b 120
madcowswe 89:dfe8c2ec5b88 121 // hack
madcowswe 89:dfe8c2ec5b88 122 if (own_colour==BLUE && i==0)
madcowswe 89:dfe8c2ec5b88 123 continue;
madcowswe 89:dfe8c2ec5b88 124
rsavitski 81:ef1ce4f5322b 125 // set new wp
rsavitski 81:ef1ce4f5322b 126 mutable_cake_wp.x = 1.5-r*cos(phi);
rsavitski 81:ef1ce4f5322b 127 mutable_cake_wp.y = 2-r*sin(phi);
rsavitski 81:ef1ce4f5322b 128 mutable_cake_wp.theta = constrainAngle(phi+PI/2);
rsavitski 81:ef1ce4f5322b 129
rsavitski 81:ef1ce4f5322b 130 //arm offset
rsavitski 81:ef1ce4f5322b 131 mutable_cake_wp.x += 0.0425*cos(mutable_cake_wp.theta);
rsavitski 81:ef1ce4f5322b 132 mutable_cake_wp.y += 0.0425*sin(mutable_cake_wp.theta);
rsavitski 81:ef1ce4f5322b 133
rsavitski 81:ef1ce4f5322b 134
rsavitski 81:ef1ce4f5322b 135 motion::waypoint_flag_mutex.lock();
rsavitski 81:ef1ce4f5322b 136 motion::setNewWaypoint(Thread::gettid(),&mutable_cake_wp);
rsavitski 81:ef1ce4f5322b 137 motion::waypoint_flag_mutex.unlock();
rsavitski 81:ef1ce4f5322b 138 Thread::signal_wait(0x1); //wait until wp reached
rsavitski 81:ef1ce4f5322b 139
rsavitski 81:ef1ce4f5322b 140 if(layer_to_push == top_layer)
rsavitski 81:ef1ce4f5322b 141 {
rsavitski 81:ef1ce4f5322b 142 ColourEnum colour_read = c_upper.getColour();
madcowswe 88:8850373c3f0d 143 if ((colour_read==own_colour || i==0) && MotorControl::motorsenabled)
rsavitski 81:ef1ce4f5322b 144 {
rsavitski 81:ef1ce4f5322b 145 arm::upper_arm.go_down();
madcowswe 88:8850373c3f0d 146 top_arm_up_timer.start(1200);
rsavitski 81:ef1ce4f5322b 147 }
madcowswe 88:8850373c3f0d 148 }
rsavitski 81:ef1ce4f5322b 149 else
rsavitski 81:ef1ce4f5322b 150 {
rsavitski 81:ef1ce4f5322b 151 ColourEnum colour_read = c_lower.getColour();
madcowswe 88:8850373c3f0d 152 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 153 {
rsavitski 81:ef1ce4f5322b 154 arm::lower_arm.go_down();
madcowswe 88:8850373c3f0d 155 bottom_arm_up_timer.start(1200);
rsavitski 81:ef1ce4f5322b 156 }
rsavitski 81:ef1ce4f5322b 157 }
madcowswe 88:8850373c3f0d 158 Thread::wait(2100);
rsavitski 81:ef1ce4f5322b 159 }
rsavitski 81:ef1ce4f5322b 160
rsavitski 81:ef1ce4f5322b 161 ////////////////////
rsavitski 77:8d83a0c00e66 162
rsavitski 78:3178a1e46146 163 while(1)
rsavitski 78:3178a1e46146 164 Thread::wait(1000);
rsavitski 30:791739422122 165 }
rsavitski 83:223186cd7531 166 #else
rsavitski 83:223186cd7531 167 enum action_t {top_push_colour, bot_push_colour, bot_push_white};
rsavitski 83:223186cd7531 168
rsavitski 83:223186cd7531 169 void ailayer(void const *dummy)
rsavitski 83:223186cd7531 170 {
rsavitski 83:223186cd7531 171 RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
rsavitski 83:223186cd7531 172 RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce);
rsavitski 83:223186cd7531 173
rsavitski 83:223186cd7531 174 ////////////////////////////////////
rsavitski 83:223186cd7531 175 // put waypoints here
rsavitski 83:223186cd7531 176 ////////////////////////////////////
rsavitski 83:223186cd7531 177 const int wp_num = 3;
rsavitski 83:223186cd7531 178
rsavitski 83:223186cd7531 179 float x_arr[wp_num] = {1.2,1.5,1.7}; //<--------------------------
rsavitski 83:223186cd7531 180 float y_arr[wp_num] = {1,1.2,1.4}; //<--------------------------
rsavitski 83:223186cd7531 181 float theta_arr[wp_num] = {0,PI, PI/2}; //<----------------------
rsavitski 83:223186cd7531 182 action_t action[wp_num] = {top_push_colour, bot_push_colour, bot_push_white}; //<----------------------------
rsavitski 83:223186cd7531 183
rsavitski 83:223186cd7531 184 Waypoint wp_arr[wp_num];
rsavitski 83:223186cd7531 185
rsavitski 83:223186cd7531 186 for(int i=0; i<wp_num; ++i)
rsavitski 83:223186cd7531 187 {
rsavitski 83:223186cd7531 188 wp_arr[i].x =x_arr[i];
rsavitski 83:223186cd7531 189 wp_arr[i].y =y_arr[i];
rsavitski 83:223186cd7531 190 wp_arr[i].theta =theta_arr[i];
rsavitski 83:223186cd7531 191
rsavitski 83:223186cd7531 192 wp_arr[i].pos_threshold = 0.01;
rsavitski 83:223186cd7531 193 wp_arr[i].angle_threshold = 0.01*PI;
rsavitski 83:223186cd7531 194 wp_arr[i].angle_exponent = 512;
rsavitski 83:223186cd7531 195 }
rsavitski 83:223186cd7531 196
rsavitski 83:223186cd7531 197 ////////////////////////////////////
rsavitski 83:223186cd7531 198
rsavitski 83:223186cd7531 199 MotorControl::motorsenabled = true;
rsavitski 83:223186cd7531 200 motion::collavoiden = 1;
rsavitski 83:223186cd7531 201
rsavitski 83:223186cd7531 202 motion::waypoint_flag_mutex.lock();
rsavitski 83:223186cd7531 203 motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
rsavitski 83:223186cd7531 204 motion::waypoint_flag_mutex.unlock();
rsavitski 83:223186cd7531 205 Thread::signal_wait(0x1); //wait until wp reached
rsavitski 83:223186cd7531 206
rsavitski 83:223186cd7531 207 MotorControl::motorsenabled = false;
rsavitski 83:223186cd7531 208 arm::upper_arm.go_up();
rsavitski 83:223186cd7531 209 arm::lower_arm.go_up();
rsavitski 83:223186cd7531 210
rsavitski 83:223186cd7531 211 Thread::signal_wait(0x2); //wait for cord
rsavitski 83:223186cd7531 212
rsavitski 83:223186cd7531 213 // CORD PULLED
rsavitski 83:223186cd7531 214 MotorControl::motorsenabled = true;
rsavitski 83:223186cd7531 215
rsavitski 83:223186cd7531 216 #ifdef TEAM_BLUE
rsavitski 83:223186cd7531 217 Waypoint mid_wp = {1.8, 1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
rsavitski 83:223186cd7531 218 motion::waypoint_flag_mutex.lock();
rsavitski 83:223186cd7531 219 motion::setNewWaypoint(Thread::gettid(),&mid_wp);
rsavitski 83:223186cd7531 220 motion::waypoint_flag_mutex.unlock();
rsavitski 83:223186cd7531 221 Thread::signal_wait(0x1); //wait until wp reached
rsavitski 83:223186cd7531 222 #endif
rsavitski 83:223186cd7531 223
rsavitski 83:223186cd7531 224 Waypoint approach_wp = {2.2, 1.85, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
rsavitski 83:223186cd7531 225 motion::waypoint_flag_mutex.lock();
rsavitski 83:223186cd7531 226 motion::setNewWaypoint(Thread::gettid(),&approach_wp);
rsavitski 83:223186cd7531 227 motion::waypoint_flag_mutex.unlock();
rsavitski 83:223186cd7531 228 Thread::signal_wait(0x1); //wait until wp reached
rsavitski 83:223186cd7531 229
rsavitski 83:223186cd7531 230
rsavitski 83:223186cd7531 231 for(int i=0; i<wp_num; ++i)
rsavitski 83:223186cd7531 232 {
rsavitski 83:223186cd7531 233 motion::waypoint_flag_mutex.lock();
rsavitski 83:223186cd7531 234 motion::setNewWaypoint(Thread::gettid(),&(wp_arr[i])); //go to home
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 switch(action[i])
rsavitski 83:223186cd7531 239 {
rsavitski 83:223186cd7531 240 case top_push_colour:
madcowswe 84:00c799fd10a7 241 if ((c_upper.getColour()==own_colour) && MotorControl::motorsenabled)
rsavitski 83:223186cd7531 242 {
rsavitski 83:223186cd7531 243 arm::upper_arm.go_down();
rsavitski 83:223186cd7531 244 top_arm_up_timer.start(1200);
rsavitski 83:223186cd7531 245 }
rsavitski 83:223186cd7531 246 break;
rsavitski 83:223186cd7531 247 case bot_push_colour:
madcowswe 84:00c799fd10a7 248 if ((c_lower.getColour()==own_colour) && MotorControl::motorsenabled)
rsavitski 83:223186cd7531 249 {
rsavitski 83:223186cd7531 250 arm::lower_arm.go_down();
rsavitski 83:223186cd7531 251 bottom_arm_up_timer.start(1200);
rsavitski 83:223186cd7531 252 }
rsavitski 83:223186cd7531 253 break;
rsavitski 83:223186cd7531 254 case bot_push_white:
madcowswe 84:00c799fd10a7 255 if (MotorControl::motorsenabled)
madcowswe 84:00c799fd10a7 256 {
madcowswe 84:00c799fd10a7 257 arm::lower_arm.go_down();
madcowswe 84:00c799fd10a7 258 bottom_arm_up_timer.start(1200);
madcowswe 84:00c799fd10a7 259 }
rsavitski 83:223186cd7531 260 break;
rsavitski 83:223186cd7531 261 }
rsavitski 83:223186cd7531 262
rsavitski 83:223186cd7531 263 Thread::wait(2200);
rsavitski 83:223186cd7531 264 }
rsavitski 83:223186cd7531 265
rsavitski 83:223186cd7531 266 while(1)
rsavitski 83:223186cd7531 267 Thread::wait(1000);
rsavitski 83:223186cd7531 268 }
rsavitski 83:223186cd7531 269 #endif
rsavitski 30:791739422122 270
rsavitski 76:532d9bc1d2aa 271 void raise_top_arm(const void *dummy)
rsavitski 70:0da6ca845762 272 {
rsavitski 70:0da6ca845762 273 arm::upper_arm.go_up();
rsavitski 70:0da6ca845762 274 }
rsavitski 70:0da6ca845762 275
rsavitski 76:532d9bc1d2aa 276 void raise_bottom_arm(const void *dummy)
rsavitski 76:532d9bc1d2aa 277 {
rsavitski 76:532d9bc1d2aa 278 arm::lower_arm.go_up();
rsavitski 76:532d9bc1d2aa 279 }
rsavitski 76:532d9bc1d2aa 280
rsavitski 69:4b7bb92916da 281 void delayed_setter(const void *tid_wpptr) //TODO: kill the hack
rsavitski 69:4b7bb92916da 282 {
rsavitski 69:4b7bb92916da 283 delayed_struct *dsptr = (delayed_struct *)tid_wpptr;
rsavitski 69:4b7bb92916da 284 motion::setNewWaypoint(dsptr->tid,dsptr->wpptr);
rsavitski 69:4b7bb92916da 285 delayed_done = true;
rsavitski 69:4b7bb92916da 286 }
rsavitski 69:4b7bb92916da 287
rsavitski 30:791739422122 288 } //namespace