This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Diff: Processes/AI/ai.cpp
- Revision:
- 83:223186cd7531
- Parent:
- 81:ef1ce4f5322b
- Child:
- 84:00c799fd10a7
--- a/Processes/AI/ai.cpp Mon Apr 15 19:49:10 2013 +0000 +++ b/Processes/AI/ai.cpp Mon Apr 15 22:28:07 2013 +0000 @@ -9,6 +9,8 @@ //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); @@ -42,14 +44,15 @@ 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); + //delayed_struct ds = {Thread::gettid(),NULL}; + //RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds); MotorControl::motorsenabled = true; motion::collavoiden = 1; @@ -149,6 +152,107 @@ 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) + { + arm::upper_arm.go_down(); + top_arm_up_timer.start(1200); + } + break; + case bot_push_colour: + if (c_lower.getColour()==own_colour) + { + arm::lower_arm.go_down(); + bottom_arm_up_timer.start(1200); + } + break; + case bot_push_white: + 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) {