From ea192b21d5b19d7bdd62fa445e5cafe1912b284b Mon Sep 17 00:00:00 2001 From: petr Date: Mon, 16 Apr 2012 21:55:08 +0200 Subject: [PATCH] robofsm: Strategy pick central buillon Prepare new strategy for competition. (Removed from file competition2012) Update Makefile for correct compilation --- src/robofsm/Makefile.omk | 5 +- src/robofsm/competition2012.cc | 254 ++------------------ src/robofsm/strategy_get_central_buillon.cc | 65 +++++ 3 files changed, 90 insertions(+), 234 deletions(-) create mode 100644 src/robofsm/strategy_get_central_buillon.cc diff --git a/src/robofsm/Makefile.omk b/src/robofsm/Makefile.omk index 6a513c01..8cc25dfb 100644 --- a/src/robofsm/Makefile.omk +++ b/src/robofsm/Makefile.omk @@ -8,8 +8,8 @@ config_include_HEADERS = robot_config.h robot_config_DEFINES = CONFIG_LOCK_CHECKING HAVE_PRIO_INHERIT bin_PROGRAMS += competition -competition_SOURCES = competition2012.cc# \ -# common-states.cc +competition_SOURCES = competition2012.cc \ + common-states.cc strategy_get_central_buillon.cc bin_PROGRAMS += homologation homologation_SOURCES = homologation2012.cc @@ -19,6 +19,7 @@ lib_LIBRARIES += robot robot_SOURCES = robot_orte.c robot.c fsmmove.cc movehelper.cc \ motion-control.cc map_handling.cc \ match-timing.c + robot_GEN_SOURCES = roboevent.c include_GEN_HEADERS += roboevent.h diff --git a/src/robofsm/competition2012.cc b/src/robofsm/competition2012.cc index 9f320d09..9ed85bb4 100644 --- a/src/robofsm/competition2012.cc +++ b/src/robofsm/competition2012.cc @@ -1,4 +1,4 @@ -/* + /* * competition.cc 12/02/28 * * Robot's control program intended for homologation (approval phase) on Eurobot 2012. @@ -24,6 +24,7 @@ #include #include "match-timing.h" #include +#include "common-states.h" UL_LOG_CUST(ulogd_homologation); /* Log domain name = ulogd + name of the file */ @@ -31,236 +32,6 @@ UL_LOG_CUST(ulogd_homologation); /* Log domain name = ulogd + name of the file * * Trajectory constraints used, are initialized in the init state ************************************************************************/ -struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow; - -/************************************************************************ - * FSM STATES DECLARATION - ************************************************************************/ - -FSM_STATE_DECL(init); -FSM_STATE_DECL(wait_for_start); -/* from homologation */ -/* movement states - buillon */ -FSM_STATE_DECL(aproach_buillon); -/* Pushing the bottle */ -FSM_STATE_DECL(push_bottle); -/* New states */ -FSM_STATE_DECL(reach_central_buillon); -FSM_STATE_DECL(leave_central_buillon); -FSM_STATE_DECL(push_bottle_bw); -FSM_STATE_DECL(return_home); - -FSM_STATE(init) -{ - switch (FSM_EVENT) { - case EV_ENTRY: - tcSlow = trajectoryConstraintsDefault; - tcSlow.maxv = 0.3; - tcSlow.maxacc = 0.3; - tcSlow.maxomega = 1; - FSM_TRANSITION(wait_for_start); - break; - default: - break; - } -} - -void set_initial_position() -{ - // TODO define initial position - robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M, - PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0), - 0); -} - -void actuators_home() -{ - //act_jaws(CLOSE); - // drive lift to home position - //act_lift(0, 0, 1); - // unset the homing request - //act_lift(0, 0, 0); -} - -FSM_STATE(wait_for_start) -{ - pthread_t thid; - #ifdef WAIT_FOR_START - ul_logdeb("WAIT_FOR_START mode set\n"); - #else - ul_logdeb("WAIT_FOR_START mode NOT set\n"); - #endif - #ifdef COMPETITION - ul_logdeb("COMPETITION mode set\n"); - #else - ul_logdeb("COMPETITION mode NOT set\n"); - #endif - switch (FSM_EVENT) { - case EV_ENTRY: - pthread_create(&thid, NULL, timing_thread, NULL); -#ifdef WAIT_FOR_START - FSM_TIMER(1000); - break; -#endif - case EV_START: - /* start competition timer */ - sem_post(&robot.start); - actuators_home(); - set_initial_position(); - FSM_TRANSITION(aproach_buillon); - break; - case EV_TIMER: - FSM_TIMER(1000); - // We set initial position periodically in - // order for it to be updated on the display - // if the team color is changed during waiting - // for start. - set_initial_position(); - if (robot.start_state == START_PLUGGED_IN) - actuators_home(); - break; - case EV_RETURN: - case EV_MOTION_ERROR: - case EV_MOTION_DONE: - //case EV_VIDLE_DONE: - case EV_SWITCH_STRATEGY: - DBG_PRINT_EVENT("unhandled event"); - break; - case EV_EXIT: - break; - default: - break; - } -} - -FSM_STATE(aproach_buillon) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - robot_trajectory_new(&tcSlow); // new trajectory - robot_trajectory_add_point_trans( - 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.5, - 1.1, - NO_TURN()); - break; - case EV_MOTION_DONE: - case EV_TIMER: - FSM_TRANSITION(reach_central_buillon); - break; - default: - break; - } -} - -FSM_STATE(push_bottle) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - robot_trajectory_new(&tcSlow); // new trajectory - robot_trajectory_add_point_trans( - 0.64, - 0.7); - robot_trajectory_add_final_point_trans( - 0.64 + 0.08, - ROBOT_AXIS_TO_FRONT_M + 0.05, - ARRIVE_FROM(DEG2RAD(270), 0.10)); - break; - case EV_MOTION_DONE: - FSM_TRANSITION(reach_central_buillon); - break; - default: - break; - } -} - -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.7); - robot_trajectory_add_point_trans( - 1.0, - 0.45); - robot_trajectory_add_final_point_trans( - 1.3, - 0.58, - NO_TURN()); - break; - case EV_MOTION_DONE: - FSM_TRANSITION(leave_central_buillon); - break; - default: - break; - } -} - -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.0, - 0.45, - NO_TURN()); - break; - case EV_MOTION_DONE: - FSM_TRANSITION(push_bottle_bw); - break; - default: - break; - } -} - -FSM_STATE(push_bottle_bw) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - robot_trajectory_new_backward(&tcSlow); // new trajectory - robot_trajectory_add_point_trans( - 0.7, - 0.3); - robot_trajectory_add_final_point_trans( - 0.64+0.08, - ROBOT_AXIS_TO_BACK_M + 0.05, - ARRIVE_FROM(DEG2RAD(270), 0.10)); - break; - case EV_MOTION_DONE: - FSM_TRANSITION(return_home); - break; - default: - break; - } -} - -FSM_STATE(return_home) -{ - switch(FSM_EVENT) { - case EV_ENTRY: - robot_trajectory_new(&tcSlow); // new trajectory - robot_trajectory_add_point_trans( - 0.64 + 0.08, - 0.7); - robot_trajectory_add_final_point_trans( - 0.4, - 1.0, - ARRIVE_FROM(DEG2RAD(180), 0.10)); - break; - case EV_MOTION_DONE: - break; - default: - break; - } -} /************************************************************************ * MAIN FUNCTION ************************************************************************/ @@ -278,10 +49,29 @@ int main() robot.fsm.motion.debug_states = 1; //robot.fsm.act.debug_states = 1; - robot.fsm.main.state = &fsm_state_main_init; + robot.fsm.main.state = &fsm_state_main_get_central_buillon_first; //robot.fsm.main.transition_callback = trans_callback; //robot.fsm.motion.transition_callback = move_trans_callback; + tcVeryFast = trajectoryConstraintsDefault; + tcVeryFast.maxv = 1; + tcVeryFast.maxacc = 0.6; + tcVeryFast.maxomega = 2; + tcFast = trajectoryConstraintsDefault; + tcFast.maxv = 0.6; + tcFast.maxacc = 0.2; + tcFast.maxomega = 1; + tcFast.maxe = 0.02; + tcSlow = trajectoryConstraintsDefault; + tcSlow.maxv = 0.4; + tcSlow.maxacc = 0.2; + tcSlow.maxomega = 1; + tcSlow.maxe = 0.02; + tcVerySlow = trajectoryConstraintsDefault; + tcVerySlow.maxv = 0.1; + tcVerySlow.maxacc = 0.1; + tcVerySlow.maxomega = 0.2; + tcVerySlow.maxe = 0.02; rv = robot_start(); if (rv) error(1, errno, "robot_start() returned %d\n", rv); diff --git a/src/robofsm/strategy_get_central_buillon.cc b/src/robofsm/strategy_get_central_buillon.cc new file mode 100644 index 00000000..5ea9b2cc --- /dev/null +++ b/src/robofsm/strategy_get_central_buillon.cc @@ -0,0 +1,65 @@ +#include "common-states.h" +#include "robot.h" +#include + + +UL_LOG_CUST(ulogd_strategy_get_central_buillon); /* Log domain name = ulogd + name of the file */ +static FSM_STATE_DECL(pick_central_buillon); + + +FSM_STATE(get_central_buillon_first) +{ + 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(pick_central_buillon); + break; + case EV_TIMER: + FSM_TIMER(1000); + start_timer(); + break; + case EV_EXIT: + start_exit(); + break; + case EV_SWITCH_STRATEGY: + //FSM_TRANSITION(ignore_central_buillon); + break; + default:; + } +} +#undef FSM_STATE_VISIBILITY +#define FSM_STATE_VISIBILITY static + +FSM_STATE_DECL(bottle_bw); + +FSM_STATE(pick_central_buillon) +{ + switch (FSM_EVENT) { + case EV_ENTRY: + SUBFSM_TRANSITION(approach_central_buillon,NULL); + break; + case EV_RETURN: + FSM_TRANSITION(bottle_bw); + break; + default:; + } +} + +FSM_STATE(bottle_bw) +{ + switch (FSM_EVENT) { + case EV_ENTRY: + SUBFSM_TRANSITION(push_bottle_bw,NULL); + break; + case EV_RETURN: + //FSM_TRANSITION(approach_totem_dowm) + default:; + } +} \ No newline at end of file -- 2.39.2