From cab8c0f1600fdf2dcd1f769c70337834f36b8c59 Mon Sep 17 00:00:00 2001
From: Filip Jares <filipjares@post.cz>
Date: Fri, 30 Apr 2010 21:49:01 +0200
Subject: [PATCH] homologation: improve the experiment with corn rushing

- use several states
- rename the experiment state to approach_next_corn
- the "central corn rushing state" is called "experiment decider"
---
 src/robofsm/homologation.cc | 115 ++++++++++++++++++++++++++++++------
 1 file changed, 96 insertions(+), 19 deletions(-)

diff --git a/src/robofsm/homologation.cc b/src/robofsm/homologation.cc
index 13f24507..7a626768 100644
--- a/src/robofsm/homologation.cc
+++ b/src/robofsm/homologation.cc
@@ -49,7 +49,10 @@ FSM_STATE_DECL(climb_the_slope);
 FSM_STATE_DECL(sledge_down);
 FSM_STATE_DECL(to_container_diag);
 FSM_STATE_DECL(to_container_ortho);
-FSM_STATE_DECL(experiment);
+FSM_STATE_DECL(experiment_decider);
+FSM_STATE_DECL(approach_next_corn);
+FSM_STATE_DECL(rush_the_corn);
+FSM_STATE_DECL(turn_around);
 
 /************************************************************************
  * INITIAL AND STARTING STATES
@@ -227,12 +230,12 @@ FSM_STATE(to_container_diag)
 			robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05));
 			break;
 		case EV_MOTION_DONE:
-			FSM_TIMER(10000);
+			FSM_TIMER(6000);
 			act_vidle(VIDLE_DOWN);
 			break;
 		case EV_TIMER:
-			//act_vidle(VIDLE_UP);
-			//FSM_TRANSITION(experiment);
+			act_vidle(VIDLE_UP);
+			FSM_TRANSITION(approach_next_corn);
 			break;
 		case EV_START:
 		case EV_RETURN:
@@ -278,22 +281,52 @@ FSM_STATE(to_container_ortho)
 	}
 }
 
-enum where_to_go {
+static enum where_to_go {
 	CORN,
-	CONTAINER
-};
+	TURN_AROUND,
+	CONTAINER,
+	NO_MORE_CORN
+} where_to_go = CORN;
+
+static struct corn *corn_to_get;
+
+FSM_STATE(experiment_decider)
+{
+	
+	switch(FSM_EVENT) {
+		case EV_ENTRY:
+			if (where_to_go == CORN) {
+				FSM_TRANSITION(approach_next_corn);
+			} else if (where_to_go == CONTAINER) {
+				FSM_TRANSITION(rush_the_corn);
+			} else if (where_to_go == TURN_AROUND) {
+				FSM_TRANSITION(turn_around);
+			} else /* NO_MORE_CORN */ { 
+			}
+			break;
+		case EV_START:
+		case EV_TIMER:
+		case EV_RETURN:
+		case EV_ACTION_DONE:
+		case EV_ACTION_ERROR:
+		case EV_MOTION_DONE:
+		case EV_GOAL_NOT_REACHABLE:
+			DBG_PRINT_EVENT("unhandled event");
+		case EV_EXIT:
+			break;
+	}
+}
 
 static int cnt = 0;
-FSM_STATE(experiment)
+FSM_STATE(approach_next_corn)
 {
-	static enum where_to_go where_to_go;
-	static struct corn *corn_to_get;
 	switch(FSM_EVENT) {
 		case EV_ENTRY: {
 				double x, y, phi;
 				robot_get_est_pos(&x, &y, &phi);
-				printf("experiment: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
+				printf("approach_next_corn: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
 					cnt, x, y, phi);
+
 				corn_to_get = choose_next_corn();
 				if (corn_to_get) {
 					Pos *p = get_corn_approach_position(corn_to_get);
@@ -302,19 +335,15 @@ FSM_STATE(experiment)
 					//robot_trajectory_add_final_point_trans(robot_trajectory_add_final_point_trans(p->x, p->y, TURN(p->phi));
 					robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast);
 					delete(p);
+					where_to_go = CONTAINER;
+				} else {
+					where_to_go = NO_MORE_CORN;
 				}
-				where_to_go = CONTAINER;
 				break;
 			}
 		case EV_MOTION_DONE:
 			cnt++;
-			if (where_to_go == CORN) {
-				FSM_TRANSITION(experiment);
-			} else {
-				remove_wall_around_corn(corn_to_get);
-				robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcFast);
-				where_to_go = CORN;
-			}
+			FSM_TRANSITION(experiment_decider);
 			break;
 		case EV_START:
 		case EV_TIMER:
@@ -328,6 +357,54 @@ FSM_STATE(experiment)
 	}
 }
 
+FSM_STATE(rush_the_corn)
+{
+	switch(FSM_EVENT) {
+		case EV_ENTRY:
+			remove_wall_around_corn(corn_to_get);
+			robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcFast);
+			where_to_go = TURN_AROUND;
+			break;
+		case EV_MOTION_DONE:
+			FSM_TRANSITION(experiment_decider);
+			break;
+		case EV_START:
+		case EV_TIMER:
+		case EV_RETURN:
+		case EV_ACTION_DONE:
+		case EV_ACTION_ERROR:
+		case EV_GOAL_NOT_REACHABLE:
+			DBG_PRINT_EVENT("unhandled event");
+		case EV_EXIT:
+			break;
+	}
+}
+
+// used to perform the maneuvre
+FSM_STATE(turn_around)
+{
+	switch(FSM_EVENT) {
+		case EV_ENTRY:
+			robot_trajectory_new_backward(&tcFast);
+			robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, TURN_CCW(90));
+			break;
+		case EV_MOTION_DONE:
+			where_to_go = CORN;
+			FSM_TRANSITION(experiment_decider);
+			break;
+		case EV_START:
+		case EV_TIMER:
+		case EV_RETURN:
+		case EV_ACTION_DONE:
+		case EV_ACTION_ERROR:
+		case EV_GOAL_NOT_REACHABLE:
+			DBG_PRINT_EVENT("unhandled event");
+		case EV_EXIT:
+			break;
+	}
+}
+
+
 /************************************************************************
  * MAIN FUNCTION
  ************************************************************************/
-- 
2.39.2