Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/AI/ai.cpp@84:00c799fd10a7, 2013-04-16 (annotated)

Committer:
madcowswe
Date:
Tue Apr 16 10:05:58 2013 +0000
Revision:
84:00c799fd10a7
Parent:
83:223186cd7531
Child:
85:b0858346d838
90s timer and armlock

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