robofsm: Competition2012

Add new state for returning back on the board.

diff --git a/src/robofsm/competition2012.cc b/src/robofsm/competition2012.cc
index 943aa577cd63d8ce63e5550d594271d99626be51..9f320d090b98276f2b5cc5bdf2fcb16721a9f6ae 100644 (file)
--- a/src/robofsm/competition2012.cc
+++ b/src/robofsm/competition2012.cc
@@ -48,6 +48,7 @@ FSM_STATE_DECL(push_bottle);
 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)
 {
@@ -233,6 +234,27 @@ FSM_STATE(push_bottle_bw)
                                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: