2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Processes/AI/ai.cpp
- Committer:
- rsavitski
- Date:
- 2013-10-15
- Revision:
- 91:fdadfd883825
- Parent:
- 90:e4164bb8c60e
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