File content as of revision 91:fdadfd883825:

//#include "ai.h"
//#include "rtos.h"
//#include "globals.h"
//#include "motion.h"
//#include "supportfuncs.h"
//#include "MotorControl.h"
//
////TODO: after 2012/2013, kill entire AI layer as it is hacked together. Rest is fine-ish
//
////#define HARDCODED_WAYPOINTS_HACK
//
//Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER);
//Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER);
//
//namespace AI
//{
//// starting position
//#ifdef TEAM_RED
//ColourEnum own_colour = RED;
//
//Waypoint home_wp = {2.7, 1.75, PI, 0.03, 0.05*PI, 32};
//#endif
//
//#ifdef TEAM_BLUE
//ColourEnum own_colour = BLUE;
//
//Waypoint home_wp = {0.3, 1, 0, 0.03, 0.05*PI, 32};
//#endif
//
//enum layer { top_layer, bot_layer };
//
//bool delayed_done = true; //TODO: kill
//
//struct delayed_struct //TODO: kill
//{
//    osThreadId tid; 
//    Waypoint *wpptr;
//};
//
//void raise_top_arm(const void *dummy);
//void raise_bottom_arm(const void *dummy);
//
//void delayed_setter(const void *tid_wpptr); //TODO: kill the hack
//
//#ifndef HARDCODED_WAYPOINTS_HACK
//void ailayer(void const *dummy)
//{
//    RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
//    RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce);
//    
//    //NB: reassign ds.wpptr before each invocation
//    //delayed_struct ds = {Thread::gettid(),NULL};
//    //RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds);
//
//    MotorControl::motorsenabled = true; 
//    motion::collavoiden = 1;
//    
//    motion::waypoint_flag_mutex.lock();
//    motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
//    motion::waypoint_flag_mutex.unlock();
//    Thread::signal_wait(0x1); //wait until wp reached
//   
//    MotorControl::motorsenabled = false;
//    arm::upper_arm.go_up();
//    arm::lower_arm.go_down();
//    
//    Thread::signal_wait(0x2); //wait for cord
//    
//    // CORD PULLED
//    MotorControl::motorsenabled = true;
//    arm::lower_arm.go_up();
//    
//    #ifdef TEAM_BLUE
//
//    Waypoint mid_wp5 = {1.2, 1, 0, 0.10, 0.15*PI, 32};
//    motion::waypoint_flag_mutex.lock();
//    motion::setNewWaypoint(Thread::gettid(),&mid_wp5);
//    motion::waypoint_flag_mutex.unlock();
//    Thread::signal_wait(0x1); //wait until wp reached
//    Thread::wait(3000);
//
//    Waypoint mid_wp = {1.9, 1.1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
//    motion::waypoint_flag_mutex.lock();
//    motion::setNewWaypoint(Thread::gettid(),&mid_wp);
//    motion::waypoint_flag_mutex.unlock();
//    Thread::signal_wait(0x1); //wait until wp reached
//    
//    Waypoint approach_wp = {2.24, 1.84/*1.85*/, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
//    motion::waypoint_flag_mutex.lock();
//    motion::setNewWaypoint(Thread::gettid(),&approach_wp); 
//    motion::waypoint_flag_mutex.unlock();
//    Thread::signal_wait(0x1); //wait until wp reached
//    #endif
//    /*
//    Waypoint approach_wp2 = {2.2-0.05, 1.83, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
//    motion::waypoint_flag_mutex.lock();
//    motion::setNewWaypoint(Thread::gettid(),&approach_wp2); 
//    motion::waypoint_flag_mutex.unlock();
//    Thread::signal_wait(0x1); //wait until wp reached*/
//    
//    Waypoint mutable_cake_wp = {0, 0, 0, 0.01, 0.01*PI, 512};
//    
//    float r = 0.26+0.35-0.0075;//-0.01; //second 0.01 for being less collisiony with sensors
//    
//    layer layer_to_push = top_layer;
//    
//    float top_target_phi = (180-(11.25/*+22.5*/))/180*PI;
//    float bot_target_phi = (180-(7.5+15))/180*PI;
//    
//    float phi = 0; // angle of vector (robot->center of cake)
//    
//    for(int i=0; i<18; ++i)
//    {
//        if (top_target_phi > bot_target_phi)
//        {
//            layer_to_push = top_layer;
//            phi = top_target_phi;
//            top_target_phi -= 22.5/180*PI;
//        }
//        else
//        {
//            layer_to_push = bot_layer;
//            phi = bot_target_phi;
//            bot_target_phi -= 15.0/180*PI;
//        }
//        
//        // hack
//        if (own_colour==BLUE && i==0)
//            continue;
//        
//        // set new wp
//        mutable_cake_wp.x = 1.5-r*cos(phi);
//        mutable_cake_wp.y = 2-r*sin(phi);
//        mutable_cake_wp.theta = constrainAngle(phi+PI/2);
//        
//        //arm offset
//        mutable_cake_wp.x += 0.0425*cos(mutable_cake_wp.theta);
//        mutable_cake_wp.y += 0.0425*sin(mutable_cake_wp.theta);
//        
//        
//        motion::waypoint_flag_mutex.lock();
//        motion::setNewWaypoint(Thread::gettid(),&mutable_cake_wp); 
//        motion::waypoint_flag_mutex.unlock();
//        Thread::signal_wait(0x1); //wait until wp reached
//        
//        if(layer_to_push == top_layer)
//        {
//            ColourEnum colour_read = c_upper.getColour();
//            if ((colour_read==own_colour || i==0) && MotorControl::motorsenabled)
//            {
//                arm::upper_arm.go_down();
//                top_arm_up_timer.start(1200);
//            }
//        }  
//        else
//        {
//            ColourEnum colour_read = c_lower.getColour();
//            if ((colour_read==own_colour || i==5+1 || i==7+1 || i==8+1 || i==10+1/*|| colour_read==WHITE*/) && MotorControl::motorsenabled)
//            {
//                arm::lower_arm.go_down();
//                bottom_arm_up_timer.start(1200);
//            }
//        }
//        Thread::wait(2100);
//    }
//    
//    ////////////////////
//    
//    while(1)
//        Thread::wait(1000);
//}
//#else
//enum action_t {top_push_colour, bot_push_colour, bot_push_white};
//
//void ailayer(void const *dummy)
//{
//    RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce);
//    RtosTimer bottom_arm_up_timer(raise_bottom_arm, osTimerOnce);
//    
//    ////////////////////////////////////
//    // put waypoints here
//    ////////////////////////////////////
//    const int wp_num = 3;
//    
//    float x_arr[wp_num] = {1.2,1.5,1.7}; //<--------------------------
//    float y_arr[wp_num] = {1,1.2,1.4}; //<--------------------------
//    float theta_arr[wp_num] = {0,PI, PI/2}; //<----------------------
//    action_t action[wp_num] = {top_push_colour, bot_push_colour, bot_push_white}; //<----------------------------
//    
//    Waypoint wp_arr[wp_num];
//
//    for(int i=0; i<wp_num; ++i)
//    {
//        wp_arr[i].x =x_arr[i];
//        wp_arr[i].y =y_arr[i];
//        wp_arr[i].theta =theta_arr[i];
//        
//        wp_arr[i].pos_threshold = 0.01;
//        wp_arr[i].angle_threshold = 0.01*PI;
//        wp_arr[i].angle_exponent = 512;  
//    }
//    
//    ////////////////////////////////////
//
//    MotorControl::motorsenabled = true; 
//    motion::collavoiden = 1;
//    
//    motion::waypoint_flag_mutex.lock();
//    motion::setNewWaypoint(Thread::gettid(),&home_wp); //go to home
//    motion::waypoint_flag_mutex.unlock();
//    Thread::signal_wait(0x1); //wait until wp reached
//   
//    MotorControl::motorsenabled = false;
//    arm::upper_arm.go_up();
//    arm::lower_arm.go_up();
//    
//    Thread::signal_wait(0x2); //wait for cord
//    
//    // CORD PULLED
//    MotorControl::motorsenabled = true;
//    
//    #ifdef TEAM_BLUE
//    Waypoint mid_wp = {1.8, 1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32};
//    motion::waypoint_flag_mutex.lock();
//    motion::setNewWaypoint(Thread::gettid(),&mid_wp);
//    motion::waypoint_flag_mutex.unlock();
//    Thread::signal_wait(0x1); //wait until wp reached
//    #endif
//    
//    Waypoint approach_wp = {2.2, 1.85, (-3.0f/4.0f)*PI, 0.03, 0.05*PI, 32};
//    motion::waypoint_flag_mutex.lock();
//    motion::setNewWaypoint(Thread::gettid(),&approach_wp); 
//    motion::waypoint_flag_mutex.unlock();
//    Thread::signal_wait(0x1); //wait until wp reached
//    
//    
//    for(int i=0; i<wp_num; ++i)
//    {
//        motion::waypoint_flag_mutex.lock();
//        motion::setNewWaypoint(Thread::gettid(),&(wp_arr[i])); //go to home
//        motion::waypoint_flag_mutex.unlock();
//        Thread::signal_wait(0x1); //wait until wp reached
//        
//        switch(action[i])
//        {
//            case top_push_colour:
//                if ((c_upper.getColour()==own_colour) && MotorControl::motorsenabled)
//                {
//                    arm::upper_arm.go_down();
//                    top_arm_up_timer.start(1200);
//                }
//            break;
//            case bot_push_colour:
//                if ((c_lower.getColour()==own_colour) && MotorControl::motorsenabled)
//                {
//                    arm::lower_arm.go_down();
//                    bottom_arm_up_timer.start(1200);
//                }
//            break;
//            case bot_push_white:
//                if (MotorControl::motorsenabled)
//                {
//                    arm::lower_arm.go_down();
//                    bottom_arm_up_timer.start(1200);
//                }
//            break;            
//        }
//        
//        Thread::wait(2200);
//    }
//    
//    while(1)
//    Thread::wait(1000);
//}
//#endif
//
//void raise_top_arm(const void *dummy)
//{
//    arm::upper_arm.go_up();
//}
//
//void raise_bottom_arm(const void *dummy)
//{
//    arm::lower_arm.go_up();
//}
//
//void delayed_setter(const void *tid_wpptr) //TODO: kill the hack
//{
//    delayed_struct *dsptr = (delayed_struct *)tid_wpptr;
//    motion::setNewWaypoint(dsptr->tid,dsptr->wpptr);
//    delayed_done = true;
//}
//
//} //namespace