Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

Processes/AI/ai.cpp@91:fdadfd883825, 2013-10-15 (annotated)

Committer:
rsavitski
Date:
Tue Oct 15 12:17:11 2013 +0000
Revision:
91:fdadfd883825
Parent:
90:e4164bb8c60e 
2014 ICRS Eurobot codebase

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rsavitski 91:fdadfd883825 1 //#include "ai.h"
rsavitski 91:fdadfd883825 2 //#include "rtos.h"
rsavitski 91:fdadfd883825 3 //#include "globals.h"
rsavitski 91:fdadfd883825 4 //#include "motion.h"
rsavitski 91:fdadfd883825 5 //#include "supportfuncs.h"
rsavitski 91:fdadfd883825 6 //#include "MotorControl.h"
rsavitski 91:fdadfd883825 7 //
rsavitski 91:fdadfd883825 8 ////TODO: after 2012/2013, kill entire AI layer as it is hacked together. Rest is fine-ish
rsavitski 91:fdadfd883825 9 //
rsavitski 91:fdadfd883825 10 ////#define HARDCODED_WAYPOINTS_HACK
rsavitski 91:fdadfd883825 11 //
rsavitski 91:fdadfd883825 12 //Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER);
rsavitski 91:fdadfd883825 13 //Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER);
rsavitski 91:fdadfd883825 14 //
rsavitski 91:fdadfd883825 15 //namespace AI
rsavitski 91:fdadfd883825 16 //{
rsavitski 91:fdadfd883825 17 //// starting position
rsavitski 91:fdadfd883825 18 //#ifdef TEAM_RED
rsavitski 91:fdadfd883825 19 //ColourEnum own_colour = RED;
rsavitski 91:fdadfd883825 20 //
rsavitski 91:fdadfd883825 21 //Waypoint home_wp = {2.7, 1.75, PI, 0.03, 0.05*PI, 32};
rsavitski 91:fdadfd883825 22 //#endif
rsavitski 91:fdadfd883825 23 //
rsavitski 91:fdadfd883825 24 //#ifdef TEAM_BLUE
rsavitski 91:fdadfd883825 25 //ColourEnum own_colour = BLUE;
rsavitski 91:fdadfd883825 26 //
rsavitski 91:fdadfd883825 27 //Waypoint home_wp = {0.3, 1, 0, 0.03, 0.05*PI, 32};
rsavitski 91:fdadfd883825 28 //#endif
rsavitski 91:fdadfd883825 29 //
rsavitski 91:fdadfd883825 30 //enum layer { top_layer, bot_layer };
rsavitski 91:fdadfd883825 31 //
rsavitski 91:fdadfd883825 32 //bool delayed_done = true; //TODO: kill
rsavitski 91:fdadfd883825 33 //
rsavitski 91:fdadfd883825 34 //struct delayed_struct //TODO: kill
rsavitski 91:fdadfd883825 35 //{
rsavitski 91:fdadfd883825 36 // osThreadId tid;
rsavitski 91:fdadfd883825 37 // Waypoint *wpptr;
rsavitski 91:fdadfd883825 38 //};
rsavitski 91:fdadfd883825 39 //
rsavitski 91:fdadfd883825 40 //void raise_top_arm(const void *dummy);
rsavitski 91:fdadfd883825 41 //void raise_bottom_arm(const void *dummy);
rsavitski 91:fdadfd883825 42 //
rsavitski 91:fdadfd883825 43 //void delayed_setter(const void *tid_wpptr); //TODO: kill the hack
rsavitski 91:fdadfd883825 44 //
rsavitski 91:fdadfd883825 45 //#ifndef HARDCODED_WAYPOINTS_HACK
rsavitski 91:fdadfd883825 46 //void ailayer(void const *dummy)
rsavitski 91:fdadfd883825 47 //{
rsavitski 91:fdadfd883825 48 // RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
rsavitski 91:fdadfd883825 49 // RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce);
rsavitski 91:fdadfd883825 50 //
rsavitski 91:fdadfd883825 51 // //NB: reassign ds.wpptr before each invocation
rsavitski 91:fdadfd883825 52 // //delayed_struct ds = {Thread::gettid(),NULL};
rsavitski 91:fdadfd883825 53 // //RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
rsavitski 91:fdadfd883825 54 //
rsavitski 91:fdadfd883825 55 // MotorControl::motorsenabled = true;
rsavitski 91:fdadfd883825 56 // motion::collavoiden = 1;
rsavitski 91:fdadfd883825 57 //
rsavitski 91:fdadfd883825 58 // motion::waypoint_flag_mutex.lock();
rsavitski 91:fdadfd883825 59 // motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
rsavitski 91:fdadfd883825 60 // motion::waypoint_flag_mutex.unlock();
rsavitski 91:fdadfd883825 61 // Thread::signal_wait(0x1); //wait until wp reached
rsavitski 91:fdadfd883825 62 //
rsavitski 91:fdadfd883825 63 // MotorControl::motorsenabled = false;
rsavitski 91:fdadfd883825 64 // arm::upper_arm.go_up();
rsavitski 91:fdadfd883825 65 // arm::lower_arm.go_down();
rsavitski 91:fdadfd883825 66 //
rsavitski 91:fdadfd883825 67 // Thread::signal_wait(0x2); //wait for cord
rsavitski 91:fdadfd883825 68 //
rsavitski 91:fdadfd883825 69 // // CORD PULLED
rsavitski 91:fdadfd883825 70 // MotorControl::motorsenabled = true;
rsavitski 91:fdadfd883825 71 // arm::lower_arm.go_up();
rsavitski 91:fdadfd883825 72 //
rsavitski 91:fdadfd883825 73 // #ifdef TEAM_BLUE
rsavitski 91:fdadfd883825 74 //
rsavitski 91:fdadfd883825 75 // Waypoint mid_wp5 = {1.2, 1, 0, 0.10, 0.15*PI, 32};
rsavitski 91:fdadfd883825 76 // motion::waypoint_flag_mutex.lock();
rsavitski 91:fdadfd883825 77 // motion::setNewWaypoint(Thread::gettid(),&mid_wp5);
rsavitski 91:fdadfd883825 78 // motion::waypoint_flag_mutex.unlock();
rsavitski 91:fdadfd883825 79 // Thread::signal_wait(0x1); //wait until wp reached
rsavitski 91:fdadfd883825 80 // Thread::wait(3000);
rsavitski 91:fdadfd883825 81 //
rsavitski 91:fdadfd883825 82 // Waypoint mid_wp = {1.9, 1.1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
rsavitski 91:fdadfd883825 83 // motion::waypoint_flag_mutex.lock();
rsavitski 91:fdadfd883825 84 // motion::setNewWaypoint(Thread::gettid(),&mid_wp);
rsavitski 91:fdadfd883825 85 // motion::waypoint_flag_mutex.unlock();
rsavitski 91:fdadfd883825 86 // Thread::signal_wait(0x1); //wait until wp reached
rsavitski 91:fdadfd883825 87 //
rsavitski 91:fdadfd883825 88 // Waypoint approach_wp = {2.24, 1.84/*1.85*/, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
rsavitski 91:fdadfd883825 89 // motion::waypoint_flag_mutex.lock();
rsavitski 91:fdadfd883825 90 // motion::setNewWaypoint(Thread::gettid(),&approach_wp);
rsavitski 91:fdadfd883825 91 // motion::waypoint_flag_mutex.unlock();
rsavitski 91:fdadfd883825 92 // Thread::signal_wait(0x1); //wait until wp reached
rsavitski 91:fdadfd883825 93 // #endif
rsavitski 91:fdadfd883825 94 // /*
rsavitski 91:fdadfd883825 95 // Waypoint approach_wp2 = {2.2-0.05, 1.83, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
rsavitski 91:fdadfd883825 96 // motion::waypoint_flag_mutex.lock();
rsavitski 91:fdadfd883825 97 // motion::setNewWaypoint(Thread::gettid(),&approach_wp2);
rsavitski 91:fdadfd883825 98 // motion::waypoint_flag_mutex.unlock();
rsavitski 91:fdadfd883825 99 // Thread::signal_wait(0x1); //wait until wp reached*/
rsavitski 91:fdadfd883825 100 //
rsavitski 91:fdadfd883825 101 // Waypoint mutable_cake_wp = {0, 0, 0, 0.01, 0.01*PI, 512};
rsavitski 91:fdadfd883825 102 //
rsavitski 91:fdadfd883825 103 // float r = 0.26+0.35-0.0075;//-0.01; //second 0.01 for being less collisiony with sensors
rsavitski 91:fdadfd883825 104 //
rsavitski 91:fdadfd883825 105 // layer layer_to_push = top_layer;
rsavitski 91:fdadfd883825 106 //
rsavitski 91:fdadfd883825 107 // float top_target_phi = (180-(11.25/*+22.5*/))/180*PI;
rsavitski 91:fdadfd883825 108 // float bot_target_phi = (180-(7.5+15))/180*PI;
rsavitski 91:fdadfd883825 109 //
rsavitski 91:fdadfd883825 110 // float phi = 0; // angle of vector (robot->center of cake)
rsavitski 91:fdadfd883825 111 //
rsavitski 91:fdadfd883825 112 // for(int i=0; i<18; ++i)
rsavitski 91:fdadfd883825 113 // {
rsavitski 91:fdadfd883825 114 // if (top_target_phi > bot_target_phi)
rsavitski 91:fdadfd883825 115 // {
rsavitski 91:fdadfd883825 116 // layer_to_push = top_layer;
rsavitski 91:fdadfd883825 117 // phi = top_target_phi;
rsavitski 91:fdadfd883825 118 // top_target_phi -= 22.5/180*PI;
rsavitski 91:fdadfd883825 119 // }
rsavitski 91:fdadfd883825 120 // else
rsavitski 91:fdadfd883825 121 // {
rsavitski 91:fdadfd883825 122 // layer_to_push = bot_layer;
rsavitski 91:fdadfd883825 123 // phi = bot_target_phi;
rsavitski 91:fdadfd883825 124 // bot_target_phi -= 15.0/180*PI;
rsavitski 91:fdadfd883825 125 // }
rsavitski 91:fdadfd883825 126 //
rsavitski 91:fdadfd883825 127 // // hack
rsavitski 91:fdadfd883825 128 // if (own_colour==BLUE && i==0)
rsavitski 91:fdadfd883825 129 // continue;
rsavitski 91:fdadfd883825 130 //
rsavitski 91:fdadfd883825 131 // // set new wp
rsavitski 91:fdadfd883825 132 // mutable_cake_wp.x = 1.5-r*cos(phi);
rsavitski 91:fdadfd883825 133 // mutable_cake_wp.y = 2-r*sin(phi);
rsavitski 91:fdadfd883825 134 // mutable_cake_wp.theta = constrainAngle(phi+PI/2);
rsavitski 91:fdadfd883825 135 //
rsavitski 91:fdadfd883825 136 // //arm offset
rsavitski 91:fdadfd883825 137 // mutable_cake_wp.x += 0.0425*cos(mutable_cake_wp.theta);
rsavitski 91:fdadfd883825 138 // mutable_cake_wp.y += 0.0425*sin(mutable_cake_wp.theta);
rsavitski 91:fdadfd883825 139 //
rsavitski 91:fdadfd883825 140 //
rsavitski 91:fdadfd883825 141 // motion::waypoint_flag_mutex.lock();
rsavitski 91:fdadfd883825 142 // motion::setNewWaypoint(Thread::gettid(),&mutable_cake_wp);
rsavitski 91:fdadfd883825 143 // motion::waypoint_flag_mutex.unlock();
rsavitski 91:fdadfd883825 144 // Thread::signal_wait(0x1); //wait until wp reached
rsavitski 91:fdadfd883825 145 //
rsavitski 91:fdadfd883825 146 // if(layer_to_push == top_layer)
rsavitski 91:fdadfd883825 147 // {
rsavitski 91:fdadfd883825 148 // ColourEnum colour_read = c_upper.getColour();
rsavitski 91:fdadfd883825 149 // if ((colour_read==own_colour || i==0) && MotorControl::motorsenabled)
rsavitski 91:fdadfd883825 150 // {
rsavitski 91:fdadfd883825 151 // arm::upper_arm.go_down();
rsavitski 91:fdadfd883825 152 // top_arm_up_timer.start(1200);
rsavitski 91:fdadfd883825 153 // }
rsavitski 91:fdadfd883825 154 // }
rsavitski 91:fdadfd883825 155 // else
rsavitski 91:fdadfd883825 156 // {
rsavitski 91:fdadfd883825 157 // ColourEnum colour_read = c_lower.getColour();
rsavitski 91:fdadfd883825 158 // if ((colour_read==own_colour || i==5+1 || i==7+1 || i==8+1 || i==10+1/*|| colour_read==WHITE*/) && MotorControl::motorsenabled)
rsavitski 91:fdadfd883825 159 // {
rsavitski 91:fdadfd883825 160 // arm::lower_arm.go_down();
rsavitski 91:fdadfd883825 161 // bottom_arm_up_timer.start(1200);
rsavitski 91:fdadfd883825 162 // }
rsavitski 91:fdadfd883825 163 // }
rsavitski 91:fdadfd883825 164 // Thread::wait(2100);
rsavitski 91:fdadfd883825 165 // }
rsavitski 91:fdadfd883825 166 //
rsavitski 91:fdadfd883825 167 // ////////////////////
rsavitski 91:fdadfd883825 168 //
rsavitski 91:fdadfd883825 169 // while(1)
rsavitski 91:fdadfd883825 170 // Thread::wait(1000);
rsavitski 91:fdadfd883825 171 //}
rsavitski 91:fdadfd883825 172 //#else
rsavitski 91:fdadfd883825 173 //enum action_t {top_push_colour, bot_push_colour, bot_push_white};
rsavitski 91:fdadfd883825 174 //
rsavitski 91:fdadfd883825 175 //void ailayer(void const *dummy)
rsavitski 91:fdadfd883825 176 //{
rsavitski 91:fdadfd883825 177 // RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
rsavitski 91:fdadfd883825 178 // RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce);
rsavitski 91:fdadfd883825 179 //
rsavitski 91:fdadfd883825 180 // ////////////////////////////////////
rsavitski 91:fdadfd883825 181 // // put waypoints here
rsavitski 91:fdadfd883825 182 // ////////////////////////////////////
rsavitski 91:fdadfd883825 183 // const int wp_num = 3;
rsavitski 91:fdadfd883825 184 //
rsavitski 91:fdadfd883825 185 // float x_arr[wp_num] = {1.2,1.5,1.7}; //<--------------------------
rsavitski 91:fdadfd883825 186 // float y_arr[wp_num] = {1,1.2,1.4}; //<--------------------------
rsavitski 91:fdadfd883825 187 // float theta_arr[wp_num] = {0,PI, PI/2}; //<----------------------
rsavitski 91:fdadfd883825 188 // action_t action[wp_num] = {top_push_colour, bot_push_colour, bot_push_white}; //<----------------------------
rsavitski 91:fdadfd883825 189 //
rsavitski 91:fdadfd883825 190 // Waypoint wp_arr[wp_num];
rsavitski 91:fdadfd883825 191 //
rsavitski 91:fdadfd883825 192 // for(int i=0; i<wp_num; ++i)
rsavitski 91:fdadfd883825 193 // {
rsavitski 91:fdadfd883825 194 // wp_arr[i].x =x_arr[i];
rsavitski 91:fdadfd883825 195 // wp_arr[i].y =y_arr[i];
rsavitski 91:fdadfd883825 196 // wp_arr[i].theta =theta_arr[i];
rsavitski 91:fdadfd883825 197 //
rsavitski 91:fdadfd883825 198 // wp_arr[i].pos_threshold = 0.01;
rsavitski 91:fdadfd883825 199 // wp_arr[i].angle_threshold = 0.01*PI;
rsavitski 91:fdadfd883825 200 // wp_arr[i].angle_exponent = 512;
rsavitski 91:fdadfd883825 201 // }
rsavitski 91:fdadfd883825 202 //
rsavitski 91:fdadfd883825 203 // ////////////////////////////////////
rsavitski 91:fdadfd883825 204 //
rsavitski 91:fdadfd883825 205 // MotorControl::motorsenabled = true;
rsavitski 91:fdadfd883825 206 // motion::collavoiden = 1;
rsavitski 91:fdadfd883825 207 //
rsavitski 91:fdadfd883825 208 // motion::waypoint_flag_mutex.lock();
rsavitski 91:fdadfd883825 209 // motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
rsavitski 91:fdadfd883825 210 // motion::waypoint_flag_mutex.unlock();
rsavitski 91:fdadfd883825 211 // Thread::signal_wait(0x1); //wait until wp reached
rsavitski 91:fdadfd883825 212 //
rsavitski 91:fdadfd883825 213 // MotorControl::motorsenabled = false;
rsavitski 91:fdadfd883825 214 // arm::upper_arm.go_up();
rsavitski 91:fdadfd883825 215 // arm::lower_arm.go_up();
rsavitski 91:fdadfd883825 216 //
rsavitski 91:fdadfd883825 217 // Thread::signal_wait(0x2); //wait for cord
rsavitski 91:fdadfd883825 218 //
rsavitski 91:fdadfd883825 219 // // CORD PULLED
rsavitski 91:fdadfd883825 220 // MotorControl::motorsenabled = true;
rsavitski 91:fdadfd883825 221 //
rsavitski 91:fdadfd883825 222 // #ifdef TEAM_BLUE
rsavitski 91:fdadfd883825 223 // Waypoint mid_wp = {1.8, 1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
rsavitski 91:fdadfd883825 224 // motion::waypoint_flag_mutex.lock();
rsavitski 91:fdadfd883825 225 // motion::setNewWaypoint(Thread::gettid(),&mid_wp);
rsavitski 91:fdadfd883825 226 // motion::waypoint_flag_mutex.unlock();
rsavitski 91:fdadfd883825 227 // Thread::signal_wait(0x1); //wait until wp reached
rsavitski 91:fdadfd883825 228 // #endif
rsavitski 91:fdadfd883825 229 //
rsavitski 91:fdadfd883825 230 // Waypoint approach_wp = {2.2, 1.85, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
rsavitski 91:fdadfd883825 231 // motion::waypoint_flag_mutex.lock();
rsavitski 91:fdadfd883825 232 // motion::setNewWaypoint(Thread::gettid(),&approach_wp);
rsavitski 91:fdadfd883825 233 // motion::waypoint_flag_mutex.unlock();
rsavitski 91:fdadfd883825 234 // Thread::signal_wait(0x1); //wait until wp reached
rsavitski 91:fdadfd883825 235 //
rsavitski 91:fdadfd883825 236 //
rsavitski 91:fdadfd883825 237 // for(int i=0; i<wp_num; ++i)
rsavitski 91:fdadfd883825 238 // {
rsavitski 91:fdadfd883825 239 // motion::waypoint_flag_mutex.lock();
rsavitski 91:fdadfd883825 240 // motion::setNewWaypoint(Thread::gettid(),&(wp_arr[i])); //go to home
rsavitski 91:fdadfd883825 241 // motion::waypoint_flag_mutex.unlock();
rsavitski 91:fdadfd883825 242 // Thread::signal_wait(0x1); //wait until wp reached
rsavitski 91:fdadfd883825 243 //
rsavitski 91:fdadfd883825 244 // switch(action[i])
rsavitski 91:fdadfd883825 245 // {
rsavitski 91:fdadfd883825 246 // case top_push_colour:
rsavitski 91:fdadfd883825 247 // if ((c_upper.getColour()==own_colour) && MotorControl::motorsenabled)
rsavitski 91:fdadfd883825 248 // {
rsavitski 91:fdadfd883825 249 // arm::upper_arm.go_down();
rsavitski 91:fdadfd883825 250 // top_arm_up_timer.start(1200);
rsavitski 91:fdadfd883825 251 // }
rsavitski 91:fdadfd883825 252 // break;
rsavitski 91:fdadfd883825 253 // case bot_push_colour:
rsavitski 91:fdadfd883825 254 // if ((c_lower.getColour()==own_colour) && MotorControl::motorsenabled)
rsavitski 91:fdadfd883825 255 // {
rsavitski 91:fdadfd883825 256 // arm::lower_arm.go_down();
rsavitski 91:fdadfd883825 257 // bottom_arm_up_timer.start(1200);
rsavitski 91:fdadfd883825 258 // }
rsavitski 91:fdadfd883825 259 // break;
rsavitski 91:fdadfd883825 260 // case bot_push_white:
rsavitski 91:fdadfd883825 261 // if (MotorControl::motorsenabled)
rsavitski 91:fdadfd883825 262 // {
rsavitski 91:fdadfd883825 263 // arm::lower_arm.go_down();
rsavitski 91:fdadfd883825 264 // bottom_arm_up_timer.start(1200);
rsavitski 91:fdadfd883825 265 // }
rsavitski 91:fdadfd883825 266 // break;
rsavitski 91:fdadfd883825 267 // }
rsavitski 91:fdadfd883825 268 //
rsavitski 91:fdadfd883825 269 // Thread::wait(2200);
rsavitski 91:fdadfd883825 270 // }
rsavitski 91:fdadfd883825 271 //
rsavitski 91:fdadfd883825 272 // while(1)
rsavitski 91:fdadfd883825 273 // Thread::wait(1000);
rsavitski 91:fdadfd883825 274 //}
rsavitski 91:fdadfd883825 275 //#endif
rsavitski 91:fdadfd883825 276 //
rsavitski 91:fdadfd883825 277 //void raise_top_arm(const void *dummy)
rsavitski 91:fdadfd883825 278 //{
rsavitski 91:fdadfd883825 279 // arm::upper_arm.go_up();
rsavitski 91:fdadfd883825 280 //}
rsavitski 91:fdadfd883825 281 //
rsavitski 91:fdadfd883825 282 //void raise_bottom_arm(const void *dummy)
rsavitski 91:fdadfd883825 283 //{
rsavitski 91:fdadfd883825 284 // arm::lower_arm.go_up();
rsavitski 91:fdadfd883825 285 //}
rsavitski 91:fdadfd883825 286 //
rsavitski 91:fdadfd883825 287 //void delayed_setter(const void *tid_wpptr) //TODO: kill the hack
rsavitski 91:fdadfd883825 288 //{
rsavitski 91:fdadfd883825 289 // delayed_struct *dsptr = (delayed_struct *)tid_wpptr;
rsavitski 91:fdadfd883825 290 // motion::setNewWaypoint(dsptr->tid,dsptr->wpptr);
rsavitski 91:fdadfd883825 291 // delayed_done = true;
rsavitski 91:fdadfd883825 292 //}
rsavitski 91:fdadfd883825 293 //
rsavitski 91:fdadfd883825 294 //} //namespace