Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/AI/ai.cpp@86:769e33a3f0ff, 2013-04-16 (annotated)

Committer:
madcowswe
Date:
Tue Apr 16 11:35:43 2013 +0000
Revision:
86:769e33a3f0ff
Parent:
85:b0858346d838
Child:
87:272a7129b04b
blue_56 works reliably

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