robofsm: competition2012

[eurobot/public.git] / src / robofsm / competition2012.cc

diff --git a/src/robofsm/competition2012.cc b/src/robofsm/competition2012.cc
index a48f94ba85ba4eb9ad79095bdb4933e41677d3fd..2c8ab1c0fd57ba49cc59ea7f36d8860643a476c0 100644 (file)
--- a/src/robofsm/competition2012.cc
+++ b/src/robofsm/competition2012.cc
@@ -49,6 +49,11 @@ FSM_STATE_DECL(leave_bottle);
 FSM_STATE_DECL(goto_totem);
 FSM_STATE_DECL(approach_totem);
 FSM_STATE_DECL(leave_totem);
+
+FSM_STATE_DECL(goto_totem2);
+FSM_STATE_DECL(approach_totem2);
+FSM_STATE_DECL(leave_totem2);
+
 FSM_STATE_DECL(go_home);
 
 FSM_STATE(init)
@@ -280,14 +285,13 @@ FSM_STATE(leave_totem)
                                NO_TURN());
                        break;
                case EV_MOTION_DONE:
-                       FSM_TRANSITION(go_home);
+                       FSM_TRANSITION(goto_totem2);
                default:
                        break;
        }
 }
 
-
-FSM_STATE(go_home)
+FSM_STATE(goto_totem2)
 {
        switch(FSM_EVENT) {
                case EV_ENTRY:
@@ -297,10 +301,62 @@ FSM_STATE(go_home)
                                0.6);
                        robot_trajectory_add_point_trans(
                                0.64,
-                               1.0);
+                               1.3);
+                       robot_trajectory_add_final_point_trans(
+                               1.1,
+                               1.6,
+                               ARRIVE_FROM(DEG2RAD(270),0.1));
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TRANSITION(approach_totem2);
+               default:
+                       break;
+       }
+}
+
+
+FSM_STATE(approach_totem2)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new(&tcSlow); // new trajectory
+                       robot_trajectory_add_final_point_trans(
+                               1.1,
+                               1.125 + ROBOT_AXIS_TO_FRONT_M + 0.05,
+                               ARRIVE_FROM(DEG2RAD(270), 0.10));
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TRANSITION(leave_totem2);
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(leave_totem2)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new_backward(&tcSlow); // new trajectory
+                       robot_trajectory_add_final_point_trans(
+                               1.1,
+                               1.6,
+                               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_final_point_trans(
-                               0.35,
-                               1.0,
+                               0.5 + ROBOT_AXIS_TO_FRONT_M,
+                               1.75,
                                ARRIVE_FROM(DEG2RAD(180),0.1));
                        break;
                case EV_MOTION_DONE: