ai.cpp

00001 #include "ai.h"
00002 #include "rtos.h"
00003 #include "globals.h"
00004 #include "motion.h"
00005 #include "Colour.h"
00006 #include "supportfuncs.h"
00007 #include "Arm.h"
00008 #include "MotorControl.h"
00009 
00010 //TODO: after 2012/2013, kill entire AI layer as it is hacked together. Rest is fine-ish
00011 
00012 //#define HARDCODED_WAYPOINTS_HACK
00013 
00014 Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER);
00015 Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER);
00016 
00017 namespace AI
00018 {
00019 // starting position
00020 #ifdef TEAM_RED
00021 ColourEnum own_colour = RED;
00022 
00023 Waypoint home_wp = {2.7, 1.75, PI, 0.03, 0.05*PI, 32};
00024 #endif
00025 
00026 #ifdef TEAM_BLUE
00027 ColourEnum own_colour = BLUE;
00028 
00029 Waypoint home_wp = {0.3, 1, 0, 0.03, 0.05*PI, 32};
00030 #endif
00031 
00032 enum layer { top_layer, bot_layer };
00033 
00034 bool delayed_done = true; //TODO: kill
00035 
00036 struct delayed_struct //TODO: kill
00037 {
00038     osThreadId tid; 
00039     Waypoint *wpptr;
00040 };
00041 
00042 void raise_top_arm(const void *dummy);
00043 void raise_bottom_arm(const void *dummy);
00044 
00045 void delayed_setter(const void *tid_wpptr); //TODO: kill the hack
00046 
00047 #ifndef HARDCODED_WAYPOINTS_HACK
00048 void ailayer(void const *dummy)
00049 {
00050     RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
00051     RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce);
00052     
00053     //NB: reassign ds.wpptr before each invocation
00054     //delayed_struct ds = {Thread::gettid(),NULL};
00055     //RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
00056 
00057     MotorControl::motorsenabled = true; 
00058     motion::collavoiden = 1;
00059     
00060     motion::waypoint_flag_mutex.lock();
00061     motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
00062     motion::waypoint_flag_mutex.unlock();
00063     Thread::signal_wait(0x1); //wait until wp reached
00064    
00065     MotorControl::motorsenabled = false;
00066     arm::upper_arm.go_up();
00067     arm::lower_arm.go_down();
00068     
00069     Thread::signal_wait(0x2); //wait for cord
00070     
00071     // CORD PULLED
00072     MotorControl::motorsenabled = true;
00073     arm::lower_arm.go_up();
00074     
00075     #ifdef TEAM_BLUE
00076 
00077     Waypoint mid_wp5 = {1.2, 1, 0, 0.10, 0.15*PI, 32};
00078     motion::waypoint_flag_mutex.lock();
00079     motion::setNewWaypoint(Thread::gettid(),&mid_wp5);
00080     motion::waypoint_flag_mutex.unlock();
00081     Thread::signal_wait(0x1); //wait until wp reached
00082     Thread::wait(3000);
00083 
00084     Waypoint mid_wp = {1.9, 1.1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
00085     motion::waypoint_flag_mutex.lock();
00086     motion::setNewWaypoint(Thread::gettid(),&mid_wp);
00087     motion::waypoint_flag_mutex.unlock();
00088     Thread::signal_wait(0x1); //wait until wp reached
00089     
00090     Waypoint approach_wp = {2.24, 1.84/*1.85*/, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
00091     motion::waypoint_flag_mutex.lock();
00092     motion::setNewWaypoint(Thread::gettid(),&approach_wp); 
00093     motion::waypoint_flag_mutex.unlock();
00094     Thread::signal_wait(0x1); //wait until wp reached
00095     #endif
00096     /*
00097     Waypoint approach_wp2 = {2.2-0.05, 1.83, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
00098     motion::waypoint_flag_mutex.lock();
00099     motion::setNewWaypoint(Thread::gettid(),&approach_wp2); 
00100     motion::waypoint_flag_mutex.unlock();
00101     Thread::signal_wait(0x1); //wait until wp reached*/
00102     
00103     Waypoint mutable_cake_wp = {0, 0, 0, 0.01, 0.01*PI, 512};
00104     
00105     float r = 0.26+0.35-0.0075;//-0.01; //second 0.01 for being less collisiony with sensors
00106     
00107     layer layer_to_push = top_layer;
00108     
00109     float top_target_phi = (180-(11.25/*+22.5*/))/180*PI;
00110     float bot_target_phi = (180-(7.5+15))/180*PI;
00111     
00112     float phi = 0; // angle of vector (robot->center of cake)
00113     
00114     for(int i=0; i<18; ++i)
00115     {
00116         if (top_target_phi > bot_target_phi)
00117         {
00118             layer_to_push = top_layer;
00119             phi = top_target_phi;
00120             top_target_phi -= 22.5/180*PI;
00121         }
00122         else
00123         {
00124             layer_to_push = bot_layer;
00125             phi = bot_target_phi;
00126             bot_target_phi -= 15.0/180*PI;
00127         }
00128         
00129         // hack
00130         if (own_colour==BLUE && i==0)
00131             continue;
00132         
00133         // set new wp
00134         mutable_cake_wp.x = 1.5-r*cos(phi);
00135         mutable_cake_wp.y = 2-r*sin(phi);
00136         mutable_cake_wp.theta = constrainAngle(phi+PI/2);
00137         
00138         //arm offset
00139         mutable_cake_wp.x += 0.0425*cos(mutable_cake_wp.theta);
00140         mutable_cake_wp.y += 0.0425*sin(mutable_cake_wp.theta);
00141         
00142         
00143         motion::waypoint_flag_mutex.lock();
00144         motion::setNewWaypoint(Thread::gettid(),&mutable_cake_wp); 
00145         motion::waypoint_flag_mutex.unlock();
00146         Thread::signal_wait(0x1); //wait until wp reached
00147         
00148         if(layer_to_push == top_layer)
00149         {
00150             ColourEnum colour_read = c_upper.getColour();
00151             if ((colour_read==own_colour || i==0) && MotorControl::motorsenabled)
00152             {
00153                 arm::upper_arm.go_down();
00154                 top_arm_up_timer.start(1200);
00155             }
00156         }  
00157         else
00158         {
00159             ColourEnum colour_read = c_lower.getColour();
00160             if ((colour_read==own_colour || i==5+1 || i==7+1 || i==8+1 || i==10+1/*|| colour_read==WHITE*/) && MotorControl::motorsenabled)
00161             {
00162                 arm::lower_arm.go_down();
00163                 bottom_arm_up_timer.start(1200);
00164             }
00165         }
00166         Thread::wait(2100);
00167     }
00168     
00169     ////////////////////
00170     
00171     while(1)
00172         Thread::wait(1000);
00173 }
00174 #else
00175 enum action_t {top_push_colour, bot_push_colour, bot_push_white};
00176 
00177 void ailayer(void const *dummy)
00178 {
00179     RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
00180     RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce);
00181     
00182     ////////////////////////////////////
00183     // put waypoints here
00184     ////////////////////////////////////
00185     const int wp_num = 3;
00186     
00187     float x_arr[wp_num] = {1.2,1.5,1.7}; //<--------------------------
00188     float y_arr[wp_num] = {1,1.2,1.4}; //<--------------------------
00189     float theta_arr[wp_num] = {0,PI, PI/2}; //<----------------------
00190     action_t action[wp_num] = {top_push_colour, bot_push_colour, bot_push_white}; //<----------------------------
00191     
00192     Waypoint wp_arr[wp_num];
00193 
00194     for(int i=0; i<wp_num; ++i)
00195     {
00196         wp_arr[i].x =x_arr[i];
00197         wp_arr[i].y =y_arr[i];
00198         wp_arr[i].theta =theta_arr[i];
00199         
00200         wp_arr[i].pos_threshold = 0.01;
00201         wp_arr[i].angle_threshold = 0.01*PI;
00202         wp_arr[i].angle_exponent = 512;  
00203     }
00204     
00205     ////////////////////////////////////
00206 
00207     MotorControl::motorsenabled = true; 
00208     motion::collavoiden = 1;
00209     
00210     motion::waypoint_flag_mutex.lock();
00211     motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
00212     motion::waypoint_flag_mutex.unlock();
00213     Thread::signal_wait(0x1); //wait until wp reached
00214    
00215     MotorControl::motorsenabled = false;
00216     arm::upper_arm.go_up();
00217     arm::lower_arm.go_up();
00218     
00219     Thread::signal_wait(0x2); //wait for cord
00220     
00221     // CORD PULLED
00222     MotorControl::motorsenabled = true;
00223     
00224     #ifdef TEAM_BLUE
00225     Waypoint mid_wp = {1.8, 1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
00226     motion::waypoint_flag_mutex.lock();
00227     motion::setNewWaypoint(Thread::gettid(),&mid_wp);
00228     motion::waypoint_flag_mutex.unlock();
00229     Thread::signal_wait(0x1); //wait until wp reached
00230     #endif
00231     
00232     Waypoint approach_wp = {2.2, 1.85, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
00233     motion::waypoint_flag_mutex.lock();
00234     motion::setNewWaypoint(Thread::gettid(),&approach_wp); 
00235     motion::waypoint_flag_mutex.unlock();
00236     Thread::signal_wait(0x1); //wait until wp reached
00237     
00238     
00239     for(int i=0; i<wp_num; ++i)
00240     {
00241         motion::waypoint_flag_mutex.lock();
00242         motion::setNewWaypoint(Thread::gettid(),&(wp_arr[i])); //go to home
00243         motion::waypoint_flag_mutex.unlock();
00244         Thread::signal_wait(0x1); //wait until wp reached
00245         
00246         switch(action[i])
00247         {
00248             case top_push_colour:
00249                 if ((c_upper.getColour()==own_colour) && MotorControl::motorsenabled)
00250                 {
00251                     arm::upper_arm.go_down();
00252                     top_arm_up_timer.start(1200);
00253                 }
00254             break;
00255             case bot_push_colour:
00256                 if ((c_lower.getColour()==own_colour) && MotorControl::motorsenabled)
00257                 {
00258                     arm::lower_arm.go_down();
00259                     bottom_arm_up_timer.start(1200);
00260                 }
00261             break;
00262             case bot_push_white:
00263                 if (MotorControl::motorsenabled)
00264                 {
00265                     arm::lower_arm.go_down();
00266                     bottom_arm_up_timer.start(1200);
00267                 }
00268             break;            
00269         }
00270         
00271         Thread::wait(2200);
00272     }
00273     
00274     while(1)
00275     Thread::wait(1000);
00276 }
00277 #endif
00278 
00279 void raise_top_arm(const void *dummy)
00280 {
00281     arm::upper_arm.go_up();
00282 }
00283 
00284 void raise_bottom_arm(const void *dummy)
00285 {
00286     arm::lower_arm.go_up();
00287 }
00288 
00289 void delayed_setter(const void *tid_wpptr) //TODO: kill the hack
00290 {
00291     delayed_struct *dsptr = (delayed_struct *)tid_wpptr;
00292     motion::setNewWaypoint(dsptr->tid,dsptr->wpptr);
00293     delayed_done = true;
00294 }
00295 
00296 } //namespace