robofsm: Strategy

Include homologation as strategy into competition machine.

diff --git a/src/robofsm/Makefile.omk b/src/robofsm/Makefile.omk
index 7d33d226542cbd27cfcd74ed71b2daf46ddd4378..7ab36b906aa22d3d22d2a949ba2db7c8a2588d16 100644 (file)
--- a/src/robofsm/Makefile.omk
+++ b/src/robofsm/Makefile.omk
@@ -10,7 +10,7 @@ robot_config_DEFINES = CONFIG_LOCK_CHECKING HAVE_PRIO_INHERIT
 bin_PROGRAMS += competition
 competition_SOURCES = competition2012.cc       \
                      common-states.cc strategy_get_central_buillon.cc \
-                     strategy_odo_calibration.cc
+                     strategy_homologation.cc strategy_odo_calibration.cc
 
 bin_PROGRAMS += homologation
 homologation_SOURCES = homologation2012.cc

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 1975f71a12168de57b250bb8ac81fa8f8e6c5528..7db13184fab6e0cf7bc067240d89f58df40d78f5 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -423,6 +423,54 @@ FSM_STATE(leave_totem_up)
        }
 }
 
+/* Homologation states */
+FSM_STATE(homolog_approach_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(homolog_push_bottle);
+                       break;
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(homolog_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:
+                       SUBFSM_RET(NULL);
+                       break;
+               default:
+                       break;
+       }
+}
+
+
 /* State for odometry calibration */
 
 FSM_STATE(go_back_for_cal)

diff --git a/src/robofsm/common-states.h b/src/robofsm/common-states.h
index 7dc1c4d068a099868e7fffa042761e7f7cb54f25..029684e818605f6029f3f21fd9c081f5f9e7eec2 100644 (file)
--- a/src/robofsm/common-states.h
+++ b/src/robofsm/common-states.h
@@ -11,7 +11,7 @@ extern double totem_x, totem_y;
 extern bool up;
 /* strategy FSM */
 FSM_STATE_DECL(get_central_buillon_first);
-FSM_STATE_DECL(ignore_central_buillon);
+FSM_STATE_DECL(homolog_wait_for_start);
 FSM_STATE_DECL(calibrate);
 
 /* Strategy catch buillon in center */
@@ -47,6 +47,11 @@ FSM_STATE_DECL(place_up_buillon);
 
 FSM_STATE_DECL(push_second_bottle);
 */
+/* HOMOLOGATION states */
+/* movement states - buillon */
+FSM_STATE_DECL(homolog_approach_buillon);
+/* Pushing the bottle */
+FSM_STATE_DECL(homolog_push_bottle);
 void start_entry();
 void start_timer();
 void start_go();

diff --git a/src/robofsm/strategy_get_central_buillon.cc b/src/robofsm/strategy_get_central_buillon.cc
index d5012c7c45fcb71626de5473351b9644b1b338ed..d82a961a8c82e95722ba56948bcd32bbae20c613 100644 (file)
--- a/src/robofsm/strategy_get_central_buillon.cc
+++ b/src/robofsm/strategy_get_central_buillon.cc
@@ -12,11 +12,11 @@ FSM_STATE(get_central_buillon_first)
        switch (FSM_EVENT) {
                case EV_ENTRY:
                        start_entry();
-#ifdef COMPETITION
+//#ifdef COMPETITION
                        ul_logmsg("waiting for start\n");
                        FSM_TIMER(1000);
                        break;
-#endif
+//#endif
                case EV_START:
                        start_go();
                        FSM_TRANSITION(pick_central_buillon);
@@ -29,7 +29,7 @@ FSM_STATE(get_central_buillon_first)
                        start_exit();
                        break;
                case EV_SWITCH_STRATEGY:
-                       FSM_TRANSITION(calibrate);
+                       FSM_TRANSITION(homolog_wait_for_start);
                        break;
                default:;
        }

diff --git a/src/robofsm/strategy_homologation.cc b/src/robofsm/strategy_homologation.cc
new file mode 100644 (file)
index 0000000..bea5415
--- /dev/null
+++ b/src/robofsm/strategy_homologation.cc
@@ -0,0 +1,37 @@
+#include "common-states.h"
+#include "robot.h"
+#include <ul_log.h>
+
+
+UL_LOG_CUST(ulogd_strategy_homologation); /* Log domain name = ulogd + name of the file */
+
+
+FSM_STATE(homolog_wait_for_start)
+{
+       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();
+                       SUBFSM_TRANSITION(homolog_approach_buillon, NULL);
+                       break;
+               case EV_TIMER:
+                       FSM_TIMER(1000);
+                       start_timer();
+                       break;
+               case EV_EXIT:
+                       start_exit();
+                       break;
+               case EV_SWITCH_STRATEGY:
+                       FSM_TRANSITION(calibrate);
+                       break;
+               case EV_RETURN:
+                       break;
+               default:;
+       }
+}
\ No newline at end of file