From cee9a72add34385f22c5bb9e43eb5a4bab9e18ba Mon Sep 17 00:00:00 2001 From: petr Date: Wed, 18 Apr 2012 19:46:48 +0200 Subject: [PATCH] robofsm: Competition strategy Finished first strategy for competition. --- src/robofsm/common-states.cc | 192 +++++++++++++++++++- src/robofsm/common-states.h | 17 +- src/robofsm/strategy_get_central_buillon.cc | 43 ++++- 3 files changed, 238 insertions(+), 14 deletions(-) diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc index d0c3b1c1..ad31ad13 100644 --- a/src/robofsm/common-states.cc +++ b/src/robofsm/common-states.cc @@ -104,7 +104,9 @@ void inline enable_bumpers(bool enabled) ************************************************************************/ struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow; - +double totem_x; +double totem_y; +bool up; FSM_STATE(approach_central_buillon) { @@ -112,15 +114,18 @@ FSM_STATE(approach_central_buillon) case EV_ENTRY: robot_trajectory_new(&tcSlow); // new trajectory robot_trajectory_add_point_trans( - 0.55, + 0.5, PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0)); + robot_trajectory_add_point_trans( + 0.64, + 1.2); robot_trajectory_add_point_trans( 0.64, 0.7); robot_trajectory_add_final_point_trans( 1.0, 0.45, - NO_TURN()); + ARRIVE_FROM(DEG2RAD(24),0.1)); // robot_trajectory_add_final_point_trans( // 1.3, // 0.58, @@ -164,8 +169,8 @@ FSM_STATE(leave_central_buillon) case EV_ENTRY: robot_trajectory_new_backward(&tcSlow); // new trajectory robot_trajectory_add_final_point_trans( - 1.0, - 0.45, + 0.85, + 0.38, NO_TURN()); break; case EV_MOTION_DONE: @@ -212,10 +217,11 @@ FSM_STATE(return_home) ARRIVE_FROM(DEG2RAD(180), 0.10)); break; case EV_MOTION_DONE: + enable_bumpers(true); FSM_TIMER(2000); break; case EV_TIMER: - FSM_TRANSITION(leave_home); + SUBFSM_RET(NULL); break; default: break; @@ -230,15 +236,189 @@ FSM_STATE(leave_home) robot_trajectory_add_final_point_trans( 0.6, 1.0, + NO_TURN()); + break; + case EV_MOTION_DONE: + FSM_TRANSITION(leave_home_for_totem); + break; + default: + break; + } +} + +FSM_STATE(leave_home_for_totem) +{ + switch(FSM_EVENT) { + case EV_ENTRY: + robot_trajectory_new(&tcSlow); // new trajectory + if(up) { + robot_trajectory_add_final_point_trans( + 0.64, + 1.3, + NO_TURN()); + } + else { + robot_trajectory_add_final_point_trans( + 0.64, + 0.7, + NO_TURN()); + } + break; + case EV_MOTION_DONE: + if(up) FSM_TRANSITION(approach_totem_up); + else FSM_TRANSITION(approach_totem_down); + break; + default: + break; + } +} + +FSM_STATE(approach_totem_down) +{ + switch(FSM_EVENT) { + case EV_ENTRY: + robot_trajectory_new(&tcSlow); // new trajectory + robot_trajectory_add_final_point_trans( + totem_x, + 0.48, TURN_CCW(DEG2RAD(90))); break; case EV_MOTION_DONE: FSM_TIMER(2000); break; + case EV_TIMER: + FSM_TRANSITION(catch_totem_buillon_down); + break; + default: + break; + } +} + +FSM_STATE(catch_totem_buillon_down) +{ + switch(FSM_EVENT) { + case EV_ENTRY: + robot_trajectory_new(&tcSlow); // new trajectory + robot_trajectory_add_final_point_trans( + totem_x, + totem_y - ROBOT_AXIS_TO_FRONT_M - 0.05, + ARRIVE_FROM(DEG2RAD(90), 0.10)); + break; + case EV_MOTION_DONE: + FSM_TIMER(2000); + break; + case EV_TIMER: + FSM_TRANSITION(leave_totem_down); + default: + break; + } +} + +FSM_STATE(leave_totem_down) +{ + switch(FSM_EVENT) { + case EV_ENTRY: + robot_trajectory_new_backward(&tcSlow); // new trajectory + robot_trajectory_add_final_point_trans( + totem_x, + 0.48, + NO_TURN()); + break; + case EV_MOTION_DONE: + FSM_TRANSITION(place_buillon_home); + default: + break; + } +} + +FSM_STATE(place_buillon_home) +{ + switch(FSM_EVENT) { + case EV_ENTRY: + robot_trajectory_new(&tcSlow); // new trajectory + if(up) { + robot_trajectory_add_point_trans( + 0.9, + 1.52); + } + else { + robot_trajectory_add_point_trans( + 0.9, + 0.48); + robot_trajectory_add_point_trans( + 0.7, + 0.8); + } + robot_trajectory_add_final_point_trans( + 0.4, + 1.0, + ARRIVE_FROM(DEG2RAD(180),0.10)); + break; + case EV_MOTION_DONE: + FSM_TIMER(2000); + break; case EV_TIMER: SUBFSM_RET(NULL); + default: + break; + } +} + +FSM_STATE(approach_totem_up) +{ + switch(FSM_EVENT) { + case EV_ENTRY: + robot_trajectory_new(&tcSlow); // new trajectory + robot_trajectory_add_final_point_trans( + totem_x, + 1.52, + TURN_CW(DEG2RAD(270))); + break; + case EV_MOTION_DONE: + FSM_TIMER(2000); + break; + case EV_TIMER: + FSM_TRANSITION(catch_totem_buillon_up); break; default: break; } +} + +FSM_STATE(catch_totem_buillon_up) +{ + switch(FSM_EVENT) { + case EV_ENTRY: + robot_trajectory_new(&tcSlow); // new trajectory + robot_trajectory_add_final_point_trans( + totem_x, + totem_y + ROBOT_AXIS_TO_FRONT_M + 0.05, + ARRIVE_FROM(DEG2RAD(270), 0.10)); + break; + case EV_MOTION_DONE: + FSM_TIMER(2000); + break; + case EV_TIMER: + FSM_TRANSITION(leave_totem_up); + break; + default: + break; + } +} + +FSM_STATE(leave_totem_up) +{ + switch(FSM_EVENT) { + case EV_ENTRY: + robot_trajectory_new_backward(&tcSlow); // new trajectory + robot_trajectory_add_final_point_trans( + totem_x, + 1.52, + NO_TURN()); + break; + case EV_MOTION_DONE: + SUBFSM_RET(NULL); + default: + break; + } } \ No newline at end of file diff --git a/src/robofsm/common-states.h b/src/robofsm/common-states.h index 8be60eb5..e17520f0 100644 --- a/src/robofsm/common-states.h +++ b/src/robofsm/common-states.h @@ -7,7 +7,8 @@ #include extern struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow; - +extern double totem_x, totem_y; +extern bool up; /* strategy FSM */ FSM_STATE_DECL(get_central_buillon_first); FSM_STATE_DECL(ignore_central_buillon); @@ -19,17 +20,23 @@ FSM_STATE_DECL(catch_central_buillon); FSM_STATE_DECL(leave_central_buillon); FSM_STATE_DECL(push_bottle_bw); FSM_STATE_DECL(return_home); -FSM_STATE_DECL(leave_home); /* Ignore central buillon */ //FSM_STATE_DECL(push_nearest_buillon); //FSM_STATE_DECL(push_bottle_fw); /* Common states */ -/*FSM_STATE_DECL(approach_totem_down); -FSM_STATE_DECL(catch_down_totem_buillon); +FSM_STATE_DECL(leave_home); +FSM_STATE_DECL(leave_home_for_totem); +FSM_STATE_DECL(approach_totem_down); +FSM_STATE_DECL(catch_totem_buillon_down); FSM_STATE_DECL(leave_totem_down); -FSM_STATE_DECL(place_down_buillon); +FSM_STATE_DECL(place_buillon_home); +FSM_STATE_DECL(approach_totem_up); +FSM_STATE_DECL(catch_totem_buillon_up); +FSM_STATE_DECL(leave_totem_up); + +/*FSM_STATE_DECL(place_down_buillon); FSM_STATE_DECL(approach_totem_up); FSM_STATE_DECL(catch_up_totem_buillon); FSM_STATE_DECL(leave_totem_up); diff --git a/src/robofsm/strategy_get_central_buillon.cc b/src/robofsm/strategy_get_central_buillon.cc index 5ea9b2cc..f380f94b 100644 --- a/src/robofsm/strategy_get_central_buillon.cc +++ b/src/robofsm/strategy_get_central_buillon.cc @@ -38,6 +38,9 @@ FSM_STATE(get_central_buillon_first) #define FSM_STATE_VISIBILITY static FSM_STATE_DECL(bottle_bw); +FSM_STATE_DECL(pick_buillon_from_totem1); +FSM_STATE_DECL(pick_buillon_from_totem2); + FSM_STATE(pick_central_buillon) { @@ -48,7 +51,8 @@ FSM_STATE(pick_central_buillon) case EV_RETURN: FSM_TRANSITION(bottle_bw); break; - default:; + default: + break; } } @@ -57,9 +61,42 @@ FSM_STATE(bottle_bw) switch (FSM_EVENT) { case EV_ENTRY: SUBFSM_TRANSITION(push_bottle_bw,NULL); + up = false; break; case EV_RETURN: - //FSM_TRANSITION(approach_totem_dowm) - default:; + FSM_TRANSITION(pick_buillon_from_totem1); + break; + default: + break; + } +} + +FSM_STATE(pick_buillon_from_totem1) +{ + switch (FSM_EVENT) { + case EV_ENTRY: + totem_x = 1.1; + totem_y = 0.875; + SUBFSM_TRANSITION(leave_home,NULL); + break; + case EV_RETURN: + FSM_TRANSITION(pick_buillon_from_totem2); + break; + default: + break; + } +} + +FSM_STATE(pick_buillon_from_totem2) +{ + switch (FSM_EVENT) { + case EV_ENTRY: + up = true; + totem_x = 1.1; + totem_y = 1.125; + SUBFSM_TRANSITION(leave_home,NULL); + break; + default: + break; } } \ No newline at end of file -- 2.39.2