ai.cpp

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