robofsm: Strategy

[eurobot/public.git] / src / robofsm / common-states.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)