From: petr <silhavik.p@gmail.com>
Date: Thu, 29 Mar 2012 11:46:53 +0000 (+0200)
Subject: robofsm: Competition2012
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/29389d7ebab177a11215aa0c53b1def1190af5c8

robofsm: Competition2012

Add new state for returning back on the board.
---

diff --git a/src/robofsm/competition2012.cc b/src/robofsm/competition2012.cc
index 943aa577..9f320d09 100644
--- 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: