robofsm: Strategy pick central buillon

Prepare new strategy for competition. (Removed from file competition2012)

Update Makefile for correct compilation

diff --git a/src/robofsm/Makefile.omk b/src/robofsm/Makefile.omk
index 6a513c0110a1d4735f1e9ed02d25b57797a8bcac..8cc25dfb550acb8af0a936c377624dfada0f7162 100644 (file)
--- 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 9f320d090b98276f2b5cc5bdf2fcb16721a9f6ae..9ed85bb45d5a5e753277f3fc940293d4cde7c510 100644 (file)
--- 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 <trgen.h>
 #include "match-timing.h"
 #include <ul_log.h>
+#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 (file)
index 0000000..5ea9b2c
--- /dev/null
+++ b/src/robofsm/strategy_get_central_buillon.cc
@@ -0,0 +1,65 @@
+#include "common-states.h"
+#include "robot.h"
+#include <ul_log.h>
+
+
+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