From 41411a52d9de1470461a980847504533c5caf25c Mon Sep 17 00:00:00 2001 From: petr Date: Sun, 25 Mar 2012 16:34:02 +0200 Subject: [PATCH] robofsm: Competition Different strategy implemented. The bottle is now pushed using backward. --- src/robofsm/competition2012.cc | 203 ++++++--------------------------- 1 file changed, 37 insertions(+), 166 deletions(-) diff --git a/src/robofsm/competition2012.cc b/src/robofsm/competition2012.cc index 2c8ab1c0..943aa577 100644 --- a/src/robofsm/competition2012.cc +++ b/src/robofsm/competition2012.cc @@ -39,22 +39,15 @@ struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow; FSM_STATE_DECL(init); FSM_STATE_DECL(wait_for_start); +/* from homologation */ /* movement states - buillon */ FSM_STATE_DECL(aproach_buillon); -FSM_STATE_DECL(place_buillon); -FSM_STATE_DECL(leave_buillon); /* Pushing the bottle */ FSM_STATE_DECL(push_bottle); -FSM_STATE_DECL(leave_bottle); -FSM_STATE_DECL(goto_totem); -FSM_STATE_DECL(approach_totem); -FSM_STATE_DECL(leave_totem); - -FSM_STATE_DECL(goto_totem2); -FSM_STATE_DECL(approach_totem2); -FSM_STATE_DECL(leave_totem2); - -FSM_STATE_DECL(go_home); +/* New states */ +FSM_STATE_DECL(reach_central_buillon); +FSM_STATE_DECL(leave_central_buillon); +FSM_STATE_DECL(push_bottle_bw); FSM_STATE(init) { @@ -145,54 +138,19 @@ FSM_STATE(aproach_buillon) case EV_ENTRY: robot_trajectory_new(&tcSlow); // new trajectory robot_trajectory_add_point_trans( - 0.62, + 0.65, PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0)); + robot_trajectory_add_point_trans( + 0.65, + 1.3); robot_trajectory_add_final_point_trans( - 0.72, - 1.14, - TURN_CW(DEG2RAD(180))); - break; - case EV_MOTION_DONE: - case EV_TIMER: - FSM_TRANSITION(place_buillon); - break; - default: - break; - } -} - -FSM_STATE(place_buillon) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - robot_trajectory_new(&tcSlow); - robot_trajectory_add_final_point_trans( - ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M, - 1.14, - NO_TURN()); - break; - case EV_MOTION_DONE: - case EV_TIMER: - FSM_TRANSITION(leave_buillon); - break; - default: - break; - } -} - -FSM_STATE(leave_buillon) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - robot_trajectory_new_backward(&tcSlow); // new trajectory - robot_trajectory_add_final_point_trans( - 0.64, - 1.14, + 0.5, + 1.1, NO_TURN()); break; case EV_MOTION_DONE: case EV_TIMER: - FSM_TRANSITION(push_bottle); + FSM_TRANSITION(reach_central_buillon); break; default: break; @@ -206,168 +164,81 @@ FSM_STATE(push_bottle) robot_trajectory_new(&tcSlow); // new trajectory robot_trajectory_add_point_trans( 0.64, - 0.9); + 0.7); robot_trajectory_add_final_point_trans( - 0.64, + 0.64 + 0.08, ROBOT_AXIS_TO_FRONT_M + 0.05, ARRIVE_FROM(DEG2RAD(270), 0.10)); break; case EV_MOTION_DONE: - FSM_TRANSITION(leave_bottle); - break; - default: - break; - } -} - -FSM_STATE(leave_bottle) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - robot_trajectory_new_backward(&tcSlow); // new trajectory - robot_trajectory_add_final_point_trans( - 0.64, - 0.4, - TURN_CCW(DEG2RAD(180))); - break; - case EV_MOTION_DONE: - case EV_TIMER: - FSM_TRANSITION(goto_totem); - break; - default: - break; - } -} - -FSM_STATE(goto_totem) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - robot_trajectory_new(&tcSlow); // new trajectory - robot_trajectory_add_final_point_trans( - 1.1, - 0.4, - TURN_CCW(DEG2RAD(90))); - break; - case EV_MOTION_DONE: - FSM_TRANSITION(approach_totem); + FSM_TRANSITION(reach_central_buillon); break; default: break; } } -FSM_STATE(approach_totem) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - robot_trajectory_new(&tcSlow); // new trajectory - robot_trajectory_add_final_point_trans( - 1.1, - 0.875 - ROBOT_AXIS_TO_FRONT_M - 0.05, - ARRIVE_FROM(DEG2RAD(90), 0.10)); - break; - case EV_MOTION_DONE: - FSM_TRANSITION(leave_totem); - default: - break; - } -} - -FSM_STATE(leave_totem) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - robot_trajectory_new_backward(&tcSlow); // new trajectory - robot_trajectory_add_final_point_trans( - 1.1, - 0.4, - NO_TURN()); - break; - case EV_MOTION_DONE: - FSM_TRANSITION(goto_totem2); - default: - break; - } -} - -FSM_STATE(goto_totem2) +FSM_STATE(reach_central_buillon) { switch(FSM_EVENT) { case EV_ENTRY: robot_trajectory_new(&tcSlow); // new trajectory robot_trajectory_add_point_trans( 0.64, - 0.6); + 0.7); robot_trajectory_add_point_trans( - 0.64, - 1.3); + 1.0, + 0.45); robot_trajectory_add_final_point_trans( - 1.1, - 1.6, - ARRIVE_FROM(DEG2RAD(270),0.1)); + 1.3, + 0.58, + NO_TURN()); break; case EV_MOTION_DONE: - FSM_TRANSITION(approach_totem2); - default: + FSM_TRANSITION(leave_central_buillon); break; - } -} - - -FSM_STATE(approach_totem2) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - robot_trajectory_new(&tcSlow); // new trajectory - robot_trajectory_add_final_point_trans( - 1.1, - 1.125 + ROBOT_AXIS_TO_FRONT_M + 0.05, - ARRIVE_FROM(DEG2RAD(270), 0.10)); - break; - case EV_MOTION_DONE: - FSM_TRANSITION(leave_totem2); default: break; } } -FSM_STATE(leave_totem2) +FSM_STATE(leave_central_buillon) { switch(FSM_EVENT) { case EV_ENTRY: robot_trajectory_new_backward(&tcSlow); // new trajectory robot_trajectory_add_final_point_trans( - 1.1, - 1.6, + 1.0, + 0.45, NO_TURN()); break; case EV_MOTION_DONE: - FSM_TRANSITION(go_home); + FSM_TRANSITION(push_bottle_bw); + break; default: break; } } -FSM_STATE(go_home) +FSM_STATE(push_bottle_bw) { switch(FSM_EVENT) { case EV_ENTRY: - robot_trajectory_new(&tcSlow); // new trajectory + robot_trajectory_new_backward(&tcSlow); // new trajectory + robot_trajectory_add_point_trans( + 0.7, + 0.3); robot_trajectory_add_final_point_trans( - 0.5 + ROBOT_AXIS_TO_FRONT_M, - 1.75, - ARRIVE_FROM(DEG2RAD(180),0.1)); + 0.64+0.08, + ROBOT_AXIS_TO_BACK_M + 0.05, + ARRIVE_FROM(DEG2RAD(270), 0.10)); break; case EV_MOTION_DONE: + break; default: break; } } - -// totem 1100 x 1000 polovička totemu 125 -// druhá láhev na 1883 - /************************************************************************ * MAIN FUNCTION ************************************************************************/ -- 2.39.2