Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/AI/ai.cpp@88:8850373c3f0d, 2013-04-16 (annotated)

Committer:
madcowswe
Date:
Tue Apr 16 12:55:10 2013 +0000
Revision:
88:8850373c3f0d
Parent:
87:272a7129b04b
Child:
89:dfe8c2ec5b88
prelim final RED

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