+ 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