From: Michal Vokac Date: Mon, 16 May 2011 16:32:30 +0000 (+0200) Subject: robofsm: Add competition sources. X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/f1cfe4bb7b700fc6d38f2ee3ae665647ae96d759 robofsm: Add competition sources. This is preliminary competition FSM with subautomatons and maybe strategy switching possibility. --- diff --git a/src/robofsm/Makefile.omk b/src/robofsm/Makefile.omk index a798acec..489842e6 100644 --- a/src/robofsm/Makefile.omk +++ b/src/robofsm/Makefile.omk @@ -7,6 +7,10 @@ default_CONFIG = CONFIG_LOCK_CHECKING=n HAVE_PRIO_INHERIT=y config_include_HEADERS = robot_config.h robot_config_DEFINES = CONFIG_LOCK_CHECKING HAVE_PRIO_INHERIT +bin_PROGRAMS += competition +competition_SOURCES = competition.cc \ + common-states.cc strategy_pick_all_our_figures.cc + bin_PROGRAMS += homologation homologation_SOURCES = homologation.cc diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc index 6bbdc483..9a9c92b2 100644 --- a/src/robofsm/common-states.cc +++ b/src/robofsm/common-states.cc @@ -11,11 +11,9 @@ #include #include #include -#include "corns_configs.h" #include "actuators.h" #include #include "match-timing.h" -#include "eb2010misc.h" #include #include @@ -34,18 +32,20 @@ UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0) -static void set_initial_position() +void set_initial_position() { - robot_set_est_pos_trans(ROBOT_AXIS_TO_FRONT_M, - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2), - DEG2RAD(180)); + robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M, + PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0), + 0); } -static void actuators_home() +void actuators_home() { - static int tmp = 0; - act_vidle(VIDLE_UP - tmp, VIDLE_FAST_SPEED); - tmp = 1 - tmp; // Force movement (we need to change the target position) + act_jaws(CLOSE); + // drive lift to home position + //act_lift(0, 0, 1); + // unset the homing request + //act_lift(0, 0, 0); } void start_entry() @@ -54,7 +54,6 @@ void start_entry() robot.check_turn_safety = false; pthread_create(&thid, NULL, timing_thread, NULL); start_timer(); - act_camera_on(); } // We set initial position periodically in order for it to be updated @@ -76,8 +75,14 @@ void start_go() void start_exit() { - robot.corns = get_all_corns(robot.corns_conf_side, robot.corns_conf_center); - act_camera_off(); + +} + +void inline enable_bumpers(bool enabled) +{ + robot.use_left_bumper = enabled; + robot.use_right_bumper = enabled; + robot.use_back_bumpers = enabled; } /************************************************************************ @@ -86,432 +91,131 @@ void start_exit() struct TrajectoryConstraints tcJerk, 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. - ************************************************************************/ - -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 { - ul_logfatal("ERROR: unknown side;"); -#warning Remove the next line - robot_exit(); - } - return ret; -} - -static struct slope_approach_style *slope_approach_style_p; - /* 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: { - slope_approach_style_p = (struct slope_approach_style *) FSM_EVENT_PTR; - if (slope_approach_style_p == NULL) { - ul_logfatal("\n\nit is not allowed to call the approach_the_slope state with NULL data!!\n\n"); -#warning remove the next line - robot_exit(); - } - 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(slope_approach_style_p->which_side, x, y); - /* if necessary, approach the slope */ - if (ready_to_climb_the_slope) { - FSM_TRANSITION(climb_the_slope); - } else { - robot_goto_trans( - x_coord(0.3, slope_approach_style_p->which_side), - PLAYGROUND_HEIGHT_M - ROBOT_WIDTH_M/2 - 0.03, - ARRIVE_FROM(DEG2RAD(0), 0.02), - &tcFast); - } - break; - } - case EV_MOTION_DONE: - FSM_TRANSITION(climb_the_slope); - break; - case EV_START: - case EV_TIMER: - case EV_RETURN: - case EV_VIDLE_DONE: - case EV_MOTION_ERROR: - case EV_SWITCH_STRATEGY: - DBG_PRINT_EVENT("unhandled event"); - case EV_EXIT: - break; - } -} - -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) -{ - struct TrajectoryConstraints tc; - switch(FSM_EVENT) { - case EV_ENTRY: { - // disables using side switches on bumpers when going up - enable_switches(false); - robot.ignore_hokuyo = true; - /* create the trajectory and go */ - tc = tcSlow; - tc.maxacc = 0.4; - robot_trajectory_new_backward(&tc); - if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_BOUNDARY) { - act_vidle(VIDLE_LOAD_PREPARE, 5); - robot_trajectory_add_point_trans( - x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side), - 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, slope_approach_style_p->which_side), - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01, - NO_TURN()); - } else if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_CENTER) { - FSM_TIMER(3800); - robot_trajectory_add_point_trans( - x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side), - //1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) + 0.01 - 1.85)); - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.199); - robot_trajectory_add_final_point_trans( - x_coord(SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.07, slope_approach_style_p->which_side), - //1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) + 0.01 - 1.85), - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.199, - NO_TURN()); - } - break; - } - case EV_MOTION_DONE: - SUBFSM_TRANSITION(load_oranges, NULL); - break; - case EV_RETURN: - FSM_TRANSITION(sledge_down); - break; - case EV_TIMER: - act_vidle(VIDLE_LOAD_PREPARE, 15); - break; - case EV_START: - case EV_VIDLE_DONE: - case EV_MOTION_ERROR: - case EV_SWITCH_STRATEGY: - DBG_PRINT_EVENT("unhandled event"); - case EV_EXIT: - break; - } -} - -/* subautomaton to load oranges in two stages */ -FSM_STATE_DECL(load_oranges2); -FSM_STATE_DECL(load_oranges3); -FSM_STATE(load_oranges) +FSM_STATE(bypass_figure_in_front_of_start) { switch(FSM_EVENT) { case EV_ENTRY: - FSM_TIMER(1000); - act_vidle(VIDLE_MIDDLE, VIDLE_MEDIUM_SPEED); - break; - case EV_VIDLE_DONE: - FSM_TIMER(1000); - break; - case EV_TIMER: - FSM_TRANSITION(load_oranges2); + robot_trajectory_new(&tcSlow); + robot_trajectory_add_point_trans( + 0.45 + 0.2, + PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)); + robot_trajectory_add_point_trans( + 0.45 + 0.35 + 0.175, + PLAYGROUND_HEIGHT_M - 0.35); + robot_trajectory_add_final_point_trans( + 0.45 + 0.35 + 0.175, + 0.7 + 0.175, + NO_TURN()); break; case EV_MOTION_DONE: - case EV_START: - case EV_RETURN: - case EV_MOTION_ERROR: - case EV_SWITCH_STRATEGY: - DBG_PRINT_EVENT("unhandled event"); - case EV_EXIT: - break; - } -} - -FSM_STATE(load_oranges2) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - act_vidle(VIDLE_UP, VIDLE_MEDIUM_SPEED); - FSM_TIMER(1000); - break; - case EV_VIDLE_DONE: - FSM_TRANSITION(load_oranges3); - //SUBFSM_RET(NULL); - break; - case EV_TIMER: - FSM_TRANSITION(load_oranges3); - //SUBFSM_RET(NULL); - break; - case EV_MOTION_DONE: - case EV_START: - case EV_RETURN: - case EV_MOTION_ERROR: - case EV_SWITCH_STRATEGY: - DBG_PRINT_EVENT("unhandled event"); - case EV_EXIT: - act_vidle(VIDLE_UP-1, VIDLE_FAST_SPEED); - break; - } -} - -FSM_STATE(load_oranges3) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - act_vidle(VIDLE_MIDDLE+50, 0); - FSM_TIMER(500); - break; - case EV_VIDLE_DONE: SUBFSM_RET(NULL); break; - case EV_TIMER: - SUBFSM_RET(NULL); - break; - case EV_MOTION_DONE: case EV_START: + case EV_TIMER: case EV_RETURN: case EV_MOTION_ERROR: case EV_SWITCH_STRATEGY: DBG_PRINT_EVENT("unhandled event"); case EV_EXIT: - act_vidle(VIDLE_UP, VIDLE_FAST_SPEED); break; } } -FSM_STATE(sledge_down) -{ - struct TrajectoryConstraints tc; +FSM_STATE(approach_second_green_figure) +{ switch(FSM_EVENT) { case EV_ENTRY: - tc = tcFast; - tc.maxe = 0.5; - robot_trajectory_new(&tc); + robot.use_left_bumper = true; + robot.use_right_bumper = true; + robot.use_back_bumpers = true; - if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_BOUNDARY) { - robot_trajectory_add_point_trans( - x_coord(1.2 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side), - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01); - robot_trajectory_add_point_trans( - x_coord(1.0 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side), - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.01); - robot_trajectory_add_point_trans( - x_coord(0.8 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side), - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.05); - robot_trajectory_add_point_trans( - x_coord(0.6 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side), - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.10); - robot_trajectory_add_final_point_trans( - x_coord(0.5 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side), - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.19, - NO_TURN()); - } else if (slope_approach_style_p->which_oranges == NEAR_PLAYGROUND_CENTER) { - robot_trajectory_add_point_trans( - x_coord(1 - ROBOT_AXIS_TO_BACK_M, slope_approach_style_p->which_side), - //1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01 - 1.85)); - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.199); - robot_trajectory_add_final_point_trans( - x_coord(SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.2, slope_approach_style_p->which_side), - //1.85 - (PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01 - 1.85), - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.199, - NO_TURN()); - } + robot_trajectory_new(&tcSlow); + robot_trajectory_add_final_point_trans( + 0.45 + 0.35, + 0.29 + 0.28, + ARRIVE_FROM(DEG2RAD(180), 0.10)); break; case EV_MOTION_DONE: - /* just for sure, try to close it one more time */ - act_vidle(VIDLE_UP, VIDLE_FAST_SPEED); - SUBFSM_RET(NULL); - delete(slope_approach_style_p); + act_jaws(OPEN); + FSM_TIMER(2000); break; - case EV_START: case EV_TIMER: + FSM_TRANSITION(load_second_green_figure); + break; case EV_RETURN: - case EV_VIDLE_DONE: case EV_MOTION_ERROR: - case EV_SWITCH_STRATEGY: - DBG_PRINT_EVENT("unhandled event"); - break; case EV_EXIT: - // enables using side switches on bumpers - enable_switches(true); - robot.ignore_hokuyo = false; - robot.check_turn_safety = true; - break; } } -/************************************************************************ - * The "unload our oranges" subautomaton - ************************************************************************/ - -FSM_STATE(to_cntainer_and_unld) +FSM_STATE(load_second_green_figure) { - TrajectoryConstraints tcNotSoFast = trajectoryConstraintsDefault; - tcNotSoFast.maxv = 0.5; tcNotSoFast.maxacc = 0.3; tcNotSoFast.maxomega = 2; switch(FSM_EVENT) { case EV_ENTRY: - robot_goto_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05), &tcNotSoFast); + robot_trajectory_new(&tcSlow); + robot_trajectory_add_final_point_trans( + ROBOT_AXIS_TO_FIGURE_CENTER_M + 0.05, + 0.29 + 0.28, + NO_TURN()); break; case EV_MOTION_DONE: - FSM_TIMER(1000); // FIXME: test this - act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED); + FSM_TIMER(2000); + act_jaws(CATCH); break; case EV_TIMER: - FSM_TRANSITION(jerk); + FSM_TRANSITION(go_out_second_green_figure); break; - case EV_START: case EV_RETURN: - case EV_VIDLE_DONE: case EV_MOTION_ERROR: - case EV_SWITCH_STRATEGY: - 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.ignore_hokuyo = false; break; } } -FSM_STATE(jerk) +FSM_STATE(go_out_second_green_figure) { - static char move_cnt; switch(FSM_EVENT) { case EV_ENTRY: - move_cnt = 0; - robot_move_by(-0.05, NO_TURN(), &tcJerk); + robot_trajectory_new_backward(&tcSlow); + robot_trajectory_add_final_point_trans(0.45 + 0.35, 0.7, NO_TURN()); break; case EV_MOTION_DONE: - if (move_cnt == 0) { - robot_move_by(0.05, NO_TURN(), &tcJerk); - } else if (move_cnt > 0) { - FSM_TIMER(1500); // FIXME: test this - } - move_cnt++; - break; case EV_TIMER: - act_vidle(VIDLE_UP, VIDLE_FAST_SPEED); - SUBFSM_RET(NULL); + FSM_TRANSITION(place_figure_to_protected_block); break; case EV_START: case EV_RETURN: - case EV_VIDLE_DONE: case EV_MOTION_ERROR: - case EV_SWITCH_STRATEGY: - DBG_PRINT_EVENT("unhandled event"); case EV_EXIT: break; } } -/************************************************************************ - * The "collect corns" subautomaton - ************************************************************************/ - -static enum where_to_go { - CORN, - TURN_AROUND, - CONTAINER, - NO_MORE_CORN -} where_to_go = CORN; - -static struct corn *corn_to_get; - -FSM_STATE(rush_corns_decider) +FSM_STATE(place_figure_to_protected_block) { switch(FSM_EVENT) { case EV_ENTRY: - if (where_to_go == CORN) { - FSM_TRANSITION(approach_next_corn); - } else if (where_to_go == CONTAINER) { - FSM_TRANSITION(rush_the_corn); - } else if (where_to_go == TURN_AROUND) { - FSM_TRANSITION(turn_around); - } else /* NO_MORE_CORN */ { - } + robot_trajectory_new(&tcSlow); + robot_trajectory_add_final_point_trans( + 0.45 + 0.2, + 0.12 + 0.08 + ROBOT_AXIS_TO_FIGURE_CENTER_M, + ARRIVE_FROM(DEG2RAD(-90), 0.20)); break; case EV_START: case EV_TIMER: - case EV_RETURN: - case EV_VIDLE_DONE: - case EV_MOTION_DONE: - case EV_MOTION_ERROR: - case EV_SWITCH_STRATEGY: - DBG_PRINT_EVENT("unhandled event"); - case EV_EXIT: - break; - } -} - -static int cnt = 0; -FSM_STATE(approach_next_corn) -{ - switch(FSM_EVENT) { - case EV_ENTRY: { - double x, y, phi; - robot_get_est_pos(&x, &y, &phi); - ul_logdeb("approach_next_corn: puck cnt: %d, est pos %.3f, %.3f, %.3f\n", - cnt, x, y, phi); - - corn_to_get = choose_next_corn(); - if (corn_to_get) { - Pos *p = get_corn_approach_position(corn_to_get); - corn_to_get->was_collected = true; - robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast); - delete(p); - where_to_go = CONTAINER; - } else { - where_to_go = NO_MORE_CORN; - } - break; - } - case EV_MOTION_DONE: - cnt++; - FSM_TRANSITION(rush_corns_decider); + FSM_TRANSITION(leave_protected_figure); break; - case EV_START: - case EV_TIMER: case EV_RETURN: - case EV_VIDLE_DONE: - case EV_MOTION_ERROR: - case EV_SWITCH_STRATEGY: - DBG_PRINT_EVENT("unhandled event"); - case EV_EXIT: - break; - } -} - -FSM_STATE(rush_the_corn) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - double x; - if (robot.team_color == BLUE) { - x = corn_to_get->position.x; - } else { - x = PLAYGROUND_WIDTH_M - corn_to_get->position.x; - } - remove_wall_around_corn(x, corn_to_get->position.y); - robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcSlow); - where_to_go = TURN_AROUND; - break; case EV_MOTION_DONE: - FSM_TRANSITION(rush_last_cms); + act_jaws(OPEN); + FSM_TIMER(2000); break; - case EV_START: - case EV_TIMER: - case EV_RETURN: - case EV_VIDLE_DONE: case EV_MOTION_ERROR: case EV_SWITCH_STRATEGY: DBG_PRINT_EVENT("unhandled event"); @@ -520,57 +224,29 @@ FSM_STATE(rush_the_corn) } } -FSM_STATE(rush_last_cms) +FSM_STATE(leave_protected_figure) { - static char move_cnt; switch(FSM_EVENT) { case EV_ENTRY: - robot.ignore_hokuyo = true; - robot.check_turn_safety = false; - move_cnt = 0; - robot_move_by(0.08, NO_TURN(), &tcSlow); - break; - case EV_MOTION_DONE: - if (move_cnt == 0) { - robot_move_by(-0.08, NO_TURN(), &tcSlow); - } else if (move_cnt > 0) { - FSM_TRANSITION(rush_corns_decider); - } - move_cnt++; + //FSM_TIMER(1000); + robot_trajectory_new_backward(&tcSlow); + robot_trajectory_add_point_trans( + 0.45 + 0.175, + 0.7); + robot_trajectory_add_final_point_trans( + 0.45 + 0.35, + 0.35 + 0.35 + 0.175, + TURN_CW(DEG2RAD(0))); break; case EV_START: case EV_TIMER: - case EV_RETURN: - case EV_VIDLE_DONE: - case EV_MOTION_ERROR: - case EV_SWITCH_STRATEGY: - DBG_PRINT_EVENT("unhandled event"); - case EV_EXIT: - robot.ignore_hokuyo = false; - robot.check_turn_safety = true; - break; - } -} - -// used to perform the maneuvre -FSM_STATE(turn_around) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - robot_trajectory_new_backward(&tcFast); - robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, TURN_CCW(90)); break; + case EV_RETURN: case EV_MOTION_DONE: - where_to_go = CORN; - FSM_TRANSITION(rush_corns_decider); + //FSM_TRANSITION(load_second_figure); + SUBFSM_RET(NULL); break; - case EV_START: - case EV_TIMER: - case EV_RETURN: - case EV_VIDLE_DONE: case EV_MOTION_ERROR: - case EV_SWITCH_STRATEGY: - DBG_PRINT_EVENT("unhandled event"); case EV_EXIT: break; } diff --git a/src/robofsm/common-states.h b/src/robofsm/common-states.h index 4d683ad8..c285bb13 100644 --- a/src/robofsm/common-states.h +++ b/src/robofsm/common-states.h @@ -8,23 +8,23 @@ extern struct TrajectoryConstraints tcJerk, tcFast, tcSlow, tcVerySlow; -FSM_STATE_DECL(start_six_oranges); -FSM_STATE_DECL(start_12_oranges); -FSM_STATE_DECL(start_opp_corn); -FSM_STATE_DECL(start_opp_oranges); - - -FSM_STATE_DECL(climb_the_slope); -FSM_STATE_DECL(load_oranges); -FSM_STATE_DECL(sledge_down); -FSM_STATE_DECL(to_cntainer_and_unld); -FSM_STATE_DECL(jerk); -FSM_STATE_DECL(rush_corns_decider); -FSM_STATE_DECL(approach_next_corn); -FSM_STATE_DECL(rush_the_corn); -FSM_STATE_DECL(rush_last_cms); -FSM_STATE_DECL(turn_around); -FSM_STATE_DECL(approach_the_slope); +FSM_STATE_DECL(start_all_our_figures); +// FSM_STATE_DECL(start_12_oranges); +// FSM_STATE_DECL(start_opp_corn); +// FSM_STATE_DECL(start_opp_oranges); +// +// +FSM_STATE_DECL(bypass_figure_in_front_of_start); +FSM_STATE_DECL(approach_second_green_figure); +FSM_STATE_DECL(load_second_green_figure); +FSM_STATE_DECL(go_out_second_green_figure); +FSM_STATE_DECL(place_figure_to_protected_block); +FSM_STATE_DECL(leave_protected_figure); +// FSM_STATE_DECL(approach_next_corn); +// FSM_STATE_DECL(rush_the_corn); +// FSM_STATE_DECL(rush_last_cms); +// FSM_STATE_DECL(turn_around); +// FSM_STATE_DECL(approach_the_slope); void start_entry(); void start_timer(); diff --git a/src/robofsm/competition.cc b/src/robofsm/competition.cc index 42a69112..bcce8f8b 100644 --- a/src/robofsm/competition.cc +++ b/src/robofsm/competition.cc @@ -1,6 +1,6 @@ /* * competition.cc 2010/04/30 - * + * * Robot's control program intended for homologation (approval phase) on Eurobot 2009. * * Copyright: (c) 2009 - 2010 CTU Dragons @@ -24,10 +24,8 @@ #include #include #include -#include "corns_configs.h" #include "actuators.h" #include "match-timing.h" -#include "eb2010misc.h" #include "common-states.h" int main() @@ -45,7 +43,7 @@ int main() //robot.fsm.main.state = &fsm_state_main_start_opp_corn; //robot.fsm.main.state = &fsm_state_main_start_opp_oranges; - robot.fsm.main.state = &fsm_state_main_start_six_oranges; + robot.fsm.main.state = &fsm_state_main_start_all_our_figures; //robot.fsm.main.transition_callback = trans_callback; //robot.fsm.motion.transition_callback = move_trans_callback; diff --git a/src/robofsm/strategy_pick_all_our_figures.cc b/src/robofsm/strategy_pick_all_our_figures.cc new file mode 100644 index 00000000..f25c1f11 --- /dev/null +++ b/src/robofsm/strategy_pick_all_our_figures.cc @@ -0,0 +1,87 @@ +#include "common-states.h" +#include "robot.h" +#include + +UL_LOG_CUST(ulogd_strategy_pick_all_our_figures); /* Log domain name = ulogd + name of the file */ + +static FSM_STATE_DECL(bypass_figure); + +FSM_STATE(start_all_our_figures) +{ + switch (FSM_EVENT) { + case EV_ENTRY: + start_entry(); +#ifdef COMPETITION + ul_logmsg("waiting for start\n"); + FSM_TIMER(1000); + break; +#endif + case EV_START: + start_go(); + FSM_TRANSITION(bypass_figure); + break; + case EV_TIMER: + FSM_TIMER(1000); + start_timer(); + break; + case EV_EXIT: + start_exit(); + break; + case EV_SWITCH_STRATEGY: + //FSM_TRANSITION(start_12_oranges); + break; + default:; + } +} + + +#undef FSM_STATE_VISIBILITY +#define FSM_STATE_VISIBILITY static + +FSM_STATE_DECL(pick_our_green_figures); +// FSM_STATE_DECL(unload_oranges); +// FSM_STATE_DECL(opp_corns); + +FSM_STATE(bypass_figure) +{ + switch (FSM_EVENT) { + case EV_ENTRY: + SUBFSM_TRANSITION(bypass_figure_in_front_of_start, NULL); + break; + case EV_RETURN: + FSM_TRANSITION(pick_our_green_figures); + break; + default: break; + } +} + +FSM_STATE(pick_our_green_figures) +{ + switch (FSM_EVENT) { + case EV_ENTRY: + SUBFSM_TRANSITION(approach_second_green_figure, NULL); + break; + case EV_RETURN: + //FSM_TRANSITION(unload_oranges); + break; + default: break; + } +} + +// FSM_STATE(unload_oranges) +// { +// switch (FSM_EVENT) { +// case EV_ENTRY: SUBFSM_TRANSITION(to_cntainer_and_unld, NULL); break; +// case EV_RETURN: FSM_TRANSITION(opp_corns); break; +// default: break; +// } +// } +// +// FSM_STATE(opp_corns) +// { +// switch (FSM_EVENT) { +// case EV_ENTRY: SUBFSM_TRANSITION(approach_next_corn, NULL); break; +// case EV_RETURN: FSM_TRANSITION(opp_corns); break; +// default: break; +// } +// } \ No newline at end of file