From 110d621fad34e10f58f39fd6c09ceee7b41ffbf8 Mon Sep 17 00:00:00 2001 From: Filip Jares Date: Sat, 22 May 2010 13:47:21 +0200 Subject: [PATCH] robofsm: basic strategy rewrite (using the skeleton created by Michal in commit e3afcb31b37) - common-states.{cc,h}: prepare states for three subautomatons which do - oranges collecting - unloading the oranges - rushing particular corns one by one - the strategy_opp_oranges is now "almost" usable --- src/robofsm/common-states.cc | 208 ++++++++++++---------------- src/robofsm/common-states.h | 6 +- src/robofsm/strategy_opp_corn.cc | 4 +- src/robofsm/strategy_opp_oranges.cc | 15 +- 4 files changed, 98 insertions(+), 135 deletions(-) diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc index 973ce830..6e977a55 100644 --- a/src/robofsm/common-states.cc +++ b/src/robofsm/common-states.cc @@ -16,9 +16,15 @@ #include #include "match-timing.h" #include "eb2010misc.h" +#include #include "common-states.h" +/************************************************************************ + * Functions used in and called from all the (almost identical) + * "wait for start" states in particular strategies. + ************************************************************************/ + static void set_initial_position() { robot_set_est_pos_trans(ROBOT_AXIS_TO_FRONT_M, @@ -31,8 +37,6 @@ static void actuators_home() act_vidle(VIDLE_UP); } - - void start_entry() { pthread_t thid; @@ -64,76 +68,54 @@ void start_exit() } /************************************************************************ - * Trajectory constraints used, are initialized in the init state + * Trajectory constraints used; They are initialized in the main() function in competition.cc ************************************************************************/ struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow; +#define VIDLE_TIMEOUT 2000 +/************************************************************************ + * States that form the "collect some oranges" subautomaton. Calling automaton + * SHOULD ALWAYS call the "approach_the_slope" state. + ************************************************************************/ -FSM_STATE(zvedej_vidle) -{ - static int cnt = 0; - switch(FSM_EVENT) { - case EV_ENTRY: - case EV_TIMER: - FSM_TIMER(500); - act_vidle((VIDLE_UP - VIDLE_DOWN)*cnt/3 + VIDLE_DOWN); - printf("--------------------cnt: %d\n", cnt); - cnt++; - if(cnt >= 3) { - robot_exit(); - //FSM_TRANSITION(sledge_down); - } - break; - case EV_START: - case EV_RETURN: - case EV_ACTION_DONE: - case EV_ACTION_ERROR: - case EV_MOTION_DONE: - case EV_GOAL_NOT_REACHABLE: - DBG_PRINT_EVENT("unhandled event"); - case EV_EXIT: - break; +bool inline is_ready_to_climb_the_slope(enum which_side which_slope, double x, double y) { + bool ret; + if (which_slope == MINE) { + ret = x < 0.5 && y > PLAYGROUND_HEIGHT_M - 0.5; + } else if (which_slope == OPPONENTS) { + ret = x > 0.5 && y > PLAYGROUND_HEIGHT_M - 0.5; + } else { + printf("ERROR: unknown side;"); robot_exit(); } + return ret; } -static enum strategy { - OPPONENTS_ORAGES_TOO, - CORNS_ONLY -} strategy = OPPONENTS_ORAGES_TOO; - static enum which_side which_slope; -static int slope_cnt = 0; + +/* assures that the robot is near the slope rim; if outside our starting area, approach the slope first */ FSM_STATE(approach_the_slope) { switch(FSM_EVENT) { - case EV_ENTRY: - if (strategy == OPPONENTS_ORAGES_TOO) { - if (slope_cnt == 0) { - which_slope = MINE; + case EV_ENTRY: { + which_slope = (enum which_side) FSM_EVENT_INT; + double x, y, phi; + robot_get_est_pos_trans(&x, &y, &phi); + /* decide */ + bool ready_to_climb_the_slope = is_ready_to_climb_the_slope(which_slope, x, y); + /* if necessary, approach the slope */ + if (ready_to_climb_the_slope) { FSM_TRANSITION(climb_the_slope); - } else if (slope_cnt == 1) { - which_slope = OPPONENTS; + } else { robot_goto_trans( - x_coord(0.3, which_slope), - PLAYGROUND_HEIGHT_M - ROBOT_WIDTH_M/2 - 0.03, - ARRIVE_FROM(DEG2RAD(0), 0.02), + x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.05, which_slope), + PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.05, + ARRIVE_FROM(DEG2RAD(0),0.05), &tcFast); - } else { - which_slope = MINE; - FSM_TRANSITION(approach_next_corn); - } - } else if (strategy == CORNS_ONLY) { - if (slope_cnt == 0) { - which_slope = MINE; - FSM_TRANSITION(climb_the_slope); - } else if (slope_cnt == 1) { - FSM_TRANSITION(approach_next_corn); } + break; } - slope_cnt++; - break; case EV_MOTION_DONE: FSM_TRANSITION(climb_the_slope); break; @@ -149,38 +131,39 @@ FSM_STATE(approach_the_slope) } } +void inline enable_switches(bool enabled) +{ + robot.use_left_switch = enabled; + robot.use_right_switch = enabled; + robot.use_back_switch = enabled; +} + FSM_STATE(climb_the_slope) { switch(FSM_EVENT) { - case EV_ENTRY: - // disables using side switches on bumpers when going up - robot.use_left_switch = false; - robot.use_right_switch = false; - robot.use_back_switch = false; - act_vidle(VIDLE_DOWN); - robot.ignore_hokuyo = true; - robot_trajectory_new_backward(&tcSlow); - if (which_slope == MINE) { - robot_trajectory_add_point_trans( - x_coord(0.5 - ROBOT_AXIS_TO_BACK_M, which_slope), - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)); - } else { + case EV_ENTRY: { + // disables using side switches on bumpers when going up + enable_switches(false); + act_vidle(VIDLE_DOWN); + robot.ignore_hokuyo = true; + /* create the trajectory and go */ + robot_trajectory_new_backward(&tcSlow); robot_trajectory_add_point_trans( x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M, which_slope), PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01); + robot_trajectory_add_final_point_trans( + x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.07, which_slope), + PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01, + NO_TURN()); + break; } - /* position for collecting oranges*/ - robot_trajectory_add_final_point_trans( - x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.07, which_slope), - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01, - NO_TURN()); - break; case EV_MOTION_DONE: - FSM_TIMER(2000); + FSM_TIMER(VIDLE_TIMEOUT); act_vidle(VIDLE_UP); break; case EV_START: case EV_TIMER: + // FIXME: respond to VIDLE EVENT FSM_TRANSITION(sledge_down); break; case EV_RETURN: @@ -201,6 +184,7 @@ FSM_STATE(sledge_down) robot_trajectory_add_point_trans( x_coord(1 - ROBOT_AXIS_TO_BACK_M, which_slope), PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01); + /* robot_trajectory_add_point_trans( x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26, which_slope), PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.10); @@ -208,11 +192,19 @@ FSM_STATE(sledge_down) x_coord(0.50, which_slope), PLAYGROUND_HEIGHT_M - 0.6, NO_TURN()); + */ + robot_trajectory_add_point_trans( + x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.20, which_slope), + PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.08); + robot_trajectory_add_final_point_trans( + x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26, which_slope), + PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.14, + NO_TURN()); break; case EV_MOTION_DONE: + /* just for sure, try to close it one more time */ act_vidle(VIDLE_UP); - FSM_TRANSITION(to_container_diag); - //FSM_TRANSITION(to_container_ortho); + SUBFSM_RET(NULL); break; case EV_START: case EV_TIMER: @@ -223,9 +215,7 @@ FSM_STATE(sledge_down) DBG_PRINT_EVENT("unhandled event"); case EV_EXIT: // enables using side switches on bumpers - robot.use_left_switch = true; - robot.use_right_switch = true; - robot.use_back_switch = true; + enable_switches(true); robot.ignore_hokuyo = false; robot.check_turn_safety = true; @@ -233,10 +223,15 @@ FSM_STATE(sledge_down) } } -FSM_STATE(to_container_diag) +/************************************************************************ + * The "unload our oranges" subautomaton + ************************************************************************/ + +FSM_STATE(to_cntainer_and_unld) { switch(FSM_EVENT) { case EV_ENTRY: + /* if (which_slope == MINE) { robot_trajectory_new(&tcFast); // face the rim with front of the robot @@ -244,18 +239,17 @@ FSM_STATE(to_container_diag) // face the rim with back of the robot robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M-0.6, 0.35); robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05)); - } else { + } else { */ robot_goto_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05), &tcFast); - } + //} break; case EV_MOTION_DONE: - FSM_TIMER(6000); + FSM_TIMER(3000); // FIXME: test this act_vidle(VIDLE_VYSIP); break; case EV_TIMER: act_vidle(VIDLE_UP); - FSM_TRANSITION(approach_the_slope); - //FSM_TRANSITION(approach_next_corn); + SUBFSM_RET(NULL); break; case EV_START: case EV_RETURN: @@ -268,38 +262,9 @@ FSM_STATE(to_container_diag) } } -FSM_STATE(to_container_ortho) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - robot_trajectory_new(&tcFast); - robot_trajectory_add_point_trans( - SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.15, - PLAYGROUND_HEIGHT_M - 0.355); - robot_trajectory_add_point_trans(0.55, PLAYGROUND_HEIGHT_M - 0.65); - robot_trajectory_add_point_trans(0.90, PLAYGROUND_HEIGHT_M - 0.75); - robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.60, PLAYGROUND_HEIGHT_M - 0.7); - robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.35, PLAYGROUND_HEIGHT_M - 0.9); - - // face the rim with front of the robot - //robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.12, ARRIVE_FROM(DEG2RAD(-90), 0.10)); - // face the rim with back of the robot - robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.40, TURN_CCW(DEG2RAD(90))); - break; - case EV_MOTION_DONE: - act_vidle(VIDLE_VYSIP); - break; - case EV_START: - case EV_TIMER: - case EV_RETURN: - case EV_ACTION_DONE: - case EV_ACTION_ERROR: - case EV_GOAL_NOT_REACHABLE: - DBG_PRINT_EVENT("unhandled event"); - case EV_EXIT: - break; - } -} +/************************************************************************ + * The "collect corns" subautomaton + ************************************************************************/ static enum where_to_go { CORN, @@ -310,9 +275,8 @@ static enum where_to_go { static struct corn *corn_to_get; -FSM_STATE(experiment_decider) +FSM_STATE(rush_corns_decider) { - switch(FSM_EVENT) { case EV_ENTRY: if (where_to_go == CORN) { @@ -363,7 +327,7 @@ FSM_STATE(approach_next_corn) } case EV_MOTION_DONE: cnt++; - FSM_TRANSITION(experiment_decider); + FSM_TRANSITION(rush_corns_decider); break; case EV_START: case EV_TIMER: @@ -392,7 +356,7 @@ FSM_STATE(rush_the_corn) where_to_go = TURN_AROUND; break; case EV_MOTION_DONE: - FSM_TRANSITION(experiment_decider); + FSM_TRANSITION(rush_corns_decider); break; case EV_START: case EV_TIMER: @@ -416,7 +380,7 @@ FSM_STATE(turn_around) break; case EV_MOTION_DONE: where_to_go = CORN; - FSM_TRANSITION(experiment_decider); + FSM_TRANSITION(rush_corns_decider); break; case EV_START: case EV_TIMER: diff --git a/src/robofsm/common-states.h b/src/robofsm/common-states.h index b4fed1ba..8e47fb77 100644 --- a/src/robofsm/common-states.h +++ b/src/robofsm/common-states.h @@ -14,13 +14,11 @@ FSM_STATE_DECL(start_opp_corn); FSM_STATE_DECL(climb_the_slope); FSM_STATE_DECL(sledge_down); -FSM_STATE_DECL(to_container_diag); -FSM_STATE_DECL(to_container_ortho); -FSM_STATE_DECL(experiment_decider); +FSM_STATE_DECL(to_cntainer_and_unld); +FSM_STATE_DECL(rush_corns_decider); FSM_STATE_DECL(approach_next_corn); FSM_STATE_DECL(rush_the_corn); FSM_STATE_DECL(turn_around); -FSM_STATE_DECL(zvedej_vidle); FSM_STATE_DECL(approach_the_slope); void start_entry(); diff --git a/src/robofsm/strategy_opp_corn.cc b/src/robofsm/strategy_opp_corn.cc index fabf6cc1..d6ce4666 100644 --- a/src/robofsm/strategy_opp_corn.cc +++ b/src/robofsm/strategy_opp_corn.cc @@ -40,7 +40,7 @@ FSM_STATE_DECL(opp_corns); FSM_STATE(pick_our_oranges) { switch (FSM_EVENT) { - case EV_ENTRY: SUBFSM_TRANSITION(climb_the_slope, NULL); break; + case EV_ENTRY: SUBFSM_TRANSITION(approach_the_slope, (void*) MINE); break; case EV_RETURN: FSM_TRANSITION(unload_oranges); break; default: break; } @@ -49,7 +49,7 @@ FSM_STATE(pick_our_oranges) FSM_STATE(unload_oranges) { switch (FSM_EVENT) { - case EV_ENTRY: SUBFSM_TRANSITION(to_container_diag, NULL); break; + case EV_ENTRY: SUBFSM_TRANSITION(to_cntainer_and_unld, NULL); break; case EV_RETURN: FSM_TRANSITION(opp_corns); break; default: break; } diff --git a/src/robofsm/strategy_opp_oranges.cc b/src/robofsm/strategy_opp_oranges.cc index 87a496a1..0a21868a 100644 --- a/src/robofsm/strategy_opp_oranges.cc +++ b/src/robofsm/strategy_opp_oranges.cc @@ -34,13 +34,13 @@ FSM_STATE(start_opp_oranges) #define FSM_STATE_VISIBILITY static FSM_STATE_DECL(unload_oranges); -FSM_STATE_DECL(opp_orange); +FSM_STATE_DECL(pick_opp_oranges); FSM_STATE_DECL(opp_corns); FSM_STATE(pick_our_oranges) { switch (FSM_EVENT) { - case EV_ENTRY: SUBFSM_TRANSITION(climb_the_slope, (void*)0); break; + case EV_ENTRY: SUBFSM_TRANSITION(approach_the_slope, (void*) MINE); break; case EV_RETURN: FSM_TRANSITION(unload_oranges); break; default: break; } @@ -49,17 +49,18 @@ FSM_STATE(pick_our_oranges) FSM_STATE(unload_oranges) { switch (FSM_EVENT) { - case EV_ENTRY: SUBFSM_TRANSITION(to_container_diag, NULL); break; - case EV_RETURN: FSM_TRANSITION(opp_orange); break; + case EV_ENTRY: SUBFSM_TRANSITION(to_cntainer_and_unld, NULL); break; + /* FIXME: do something more meaningfull the next time */ + case EV_RETURN: FSM_TRANSITION(pick_opp_oranges); break; default: break; } } -FSM_STATE(opp_orange) +FSM_STATE(pick_opp_oranges) { switch (FSM_EVENT) { - case EV_ENTRY: SUBFSM_TRANSITION(climb_the_slope, (void*)1); break; - case EV_RETURN: FSM_TRANSITION(opp_corns); break; + case EV_ENTRY: SUBFSM_TRANSITION(approach_the_slope, (void*) OPPONENTS); break; + case EV_RETURN: FSM_TRANSITION(unload_oranges); break; default: break; } } -- 2.39.2