2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
Diff: Processes/AI/ai.cpp
- Revision:
- 81:ef1ce4f5322b
- Parent:
- 78:3178a1e46146
- Child:
- 83:223186cd7531
--- a/Processes/AI/ai.cpp Mon Apr 15 17:07:40 2013 +0000 +++ b/Processes/AI/ai.cpp Mon Apr 15 19:35:29 2013 +0000 @@ -27,6 +27,8 @@ 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 @@ -52,98 +54,100 @@ 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 - - Waypoint mid_wp = {1.8, 1, (1.0/4.0)*PI, 0.03, 0.05*PI, 32}; + MotorControl::motorsenabled = true; - 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 - // TODO: ?(disable collison avoidance), approach first cake wp, enable collision - /* - ///////////////temp hack - Waypoint temp_wp = {1.222,1.440, 149.0/180*PI, 0.01, 0.02*PI, 512}; - motion::setNewWaypoint(Thread::gettid(),&temp_wp); - Thread::signal_wait(0x1); //wait until wp reached - arm::lower_arm.go_down(); - bottom_arm_up_timer.start(1200); - /////////////// temp hack over - */ + Waypoint mutable_cake_wp = {0, 0, 0, 0.01, 0.01*PI, 512}; + + float r = 0.26+0.35+0.01+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<17; ++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; + } + + // 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) + { + 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 || i==7 || i==8 || i==10/*|| colour_read==WHITE*/) + { + arm::lower_arm.go_down(); + bottom_arm_up_timer.start(1200); + } + } + Thread::wait(2200); + } + + //////////////////// while(1) Thread::wait(1000); - /* - /////////////////////////////////////////////////////// - - // first waypoint for approach - current_waypoint.x = 2.2; - current_waypoint.y = 1.85; - current_waypoint.theta = (-3.0f/4.0f)*PI; - current_waypoint.pos_threshold = 0.03; - current_waypoint.angle_threshold = 0.02*PI; - - motion::collavoiden = 1; - MotorControl::motorsenabled = 1; - motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); - - float r = 0.26+0.35+0.02+0.01; - - bool firstavoidstop = 1; - delayed_struct ds = {Thread::gettid(),¤t_waypoint}; - RtosTimer delayed_wp_set(delayed_setter, osTimerOnce, (void *)&ds); - RtosTimer top_arm_up_timer(raise_top_arm, osTimerOnce); - - for (float phi=(180-11.25)/180*PI; phi > 11.25/180*PI;) - { - motion::waypoint_flag_mutex.lock(); - if (motion::checkMotionStatus() && (c_upper.getColour()==own_colour || firstavoidstop) && delayed_done) - { - //temphack!!!!! - //Thread::wait(1000); - arm::upper_arm.go_down(); - delayed_done = false; - top_arm_up_timer.start(1200); - delayed_wp_set.start(2400); - //Thread::wait(2000); - //arm::upper_arm.go_up(); - - phi -= 22.5/180*PI; - current_waypoint.x = 1.5-r*cos(phi); - current_waypoint.y = 2-r*sin(phi); - current_waypoint.theta = constrainAngle(phi+PI/2); - - //arm offset - current_waypoint.x += 0.0425*cos(current_waypoint.theta); - current_waypoint.y += 0.0425*sin(current_waypoint.theta); - - current_waypoint.pos_threshold = 0.01; - current_waypoint.angle_threshold = 0.01*PI; - - //motion::setNewWaypoint(Thread::gettid(),¤t_waypoint); - if (firstavoidstop) - { - motion::collavoiden = 0; - firstavoidstop = 0; - } - else - motion::collavoiden = 1; - } - motion::waypoint_flag_mutex.unlock(); - - Thread::wait(50); - }*/ } void raise_top_arm(const void *dummy)