From 8406cc8f25346ffa8ac6d6d31d87bf18e63db090 Mon Sep 17 00:00:00 2001 From: petr Date: Thu, 15 Mar 2012 21:40:13 +0100 Subject: [PATCH] robofsm: Competition 2012 Simple competition state machine. Go to the nearest totem and go back on the boat. --- src/robofsm/competition2012.cc | 342 +++++++++++++++++++++++++++++++++ 1 file changed, 342 insertions(+) create mode 100644 src/robofsm/competition2012.cc diff --git a/src/robofsm/competition2012.cc b/src/robofsm/competition2012.cc new file mode 100644 index 00000000..a48f94ba --- /dev/null +++ b/src/robofsm/competition2012.cc @@ -0,0 +1,342 @@ +/* + * competition.cc 12/02/28 + * + * Robot's control program intended for homologation (approval phase) on Eurobot 2012. + * + * Copyright: (c) 2012 CTU Flamingos + * CTU FEE - Department of Control Engineering + * License: GNU GPL v.2 + */ + +#define FSM_MAIN +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "actuators.h" +#include +#include "match-timing.h" +#include + +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); +/* 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(go_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.62, + PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0)); + 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, + NO_TURN()); + break; + case EV_MOTION_DONE: + case EV_TIMER: + FSM_TRANSITION(push_bottle); + 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.9); + robot_trajectory_add_final_point_trans( + 0.64, + 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); + 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(go_home); + default: + break; + } +} + + +FSM_STATE(go_home) +{ + switch(FSM_EVENT) { + case EV_ENTRY: + robot_trajectory_new(&tcSlow); // new trajectory + robot_trajectory_add_point_trans( + 0.64, + 0.6); + robot_trajectory_add_point_trans( + 0.64, + 1.0); + robot_trajectory_add_final_point_trans( + 0.35, + 1.0, + ARRIVE_FROM(DEG2RAD(180),0.1)); + break; + case EV_MOTION_DONE: + default: + break; + } +} + +// totem 1100 x 1000 polovička totemu 125 +// druhá láhev na 1883 + +/************************************************************************ + * MAIN FUNCTION + ************************************************************************/ + +int main() +{ + int rv; + + rv = robot_init(); + if (rv) error(1, errno, "robot_init() returned %d\n", rv); + + robot.obstacle_avoidance_enabled = true; + + robot.fsm.main.debug_states = 1; + robot.fsm.motion.debug_states = 1; + //robot.fsm.act.debug_states = 1; + + robot.fsm.main.state = &fsm_state_main_init; + //robot.fsm.main.transition_callback = trans_callback; + //robot.fsm.motion.transition_callback = move_trans_callback; + + rv = robot_start(); + if (rv) error(1, errno, "robot_start() returned %d\n", rv); + + robot_destroy(); + + return 0; +} \ No newline at end of file -- 2.39.2