From efc04aa6e73f50e6b9b33ffa40f9dcd756b16fb1 Mon Sep 17 00:00:00 2001
From: Michal Sojka <sojkam1@fel.cvut.cz>
Date: Sat, 3 May 2008 16:15:48 +0200
Subject: [PATCH] robofsm: Fixed initialization and added possibility to
 deposit before competition

---
 src/robofsm/eb2008/fsmmain.cc   | 52 ++++++++++++++++++++++++++-------
 src/robofsm/eb2008/robot_orte.c | 15 +++++++---
 2 files changed, 53 insertions(+), 14 deletions(-)

diff --git a/src/robofsm/eb2008/fsmmain.cc b/src/robofsm/eb2008/fsmmain.cc
index e40ecbf9..87b489cc 100644
--- a/src/robofsm/eb2008/fsmmain.cc
+++ b/src/robofsm/eb2008/fsmmain.cc
@@ -259,6 +259,8 @@ void move_trans_callback(struct robo_fsm *fsm)
 
 /* initial and starting states */
 FSM_STATE_DECL(init);
+FSM_STATE_DECL(wait_for_deposit);
+FSM_STATE_DECL(wait_for_laser);
 FSM_STATE_DECL(wait_for_start);
 FSM_STATE_DECL(decide_where_now);
 /* movement states */
@@ -281,7 +283,7 @@ FSM_STATE(init)
 {
 	switch (FSM_EVENT) {
 		case EV_ENTRY:
-			FSM_TRANSITION(wait_for_start);
+			FSM_TRANSITION(wait_for_laser);
 			break;
 		default:
 			break;
@@ -290,9 +292,12 @@ FSM_STATE(init)
 
 FSM_STATE(wait_for_laser) 
 {
-	/* start event ocurred */
 	switch (FSM_EVENT) {
+#ifdef WAIT_FOR_START
+		case EV_START:
+#else
 		case EV_ENTRY:
+#endif		
 			robot_set_est_pos_trans(ROBOT_WIDTH_M/2,
 						PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
 						DEG2RAD(-45));
@@ -301,23 +306,47 @@ FSM_STATE(wait_for_laser)
 			brushes_out();
 			open_bottom_door();
 			close_back_door();
-			FSM_TIMER(2000);
-			break;
-		case EV_START_LASER:
 			robot_set_est_pos_trans(ROBOT_WIDTH_M/2,
 						PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
 						DEG2RAD(-45));
+
+			robot.orte.laser_cmd.speed = LASER_DRIVE_ON;
+			ORTEPublicationSend(robot.orte.publication_laser_cmd);
 			robot.orte.laser_cmd.speed = LASER_DRIVE_ON;
 			ORTEPublicationSend(robot.orte.publication_laser_cmd);
 			robot.orte.laser_cmd.speed = LASER_DRIVE_ON;
 			ORTEPublicationSend(robot.orte.publication_laser_cmd);
+
+#ifdef WAIT_FOR_START			
+			FSM_TRANSITION(wait_for_deposit);
+#else
 			FSM_TRANSITION(wait_for_start);
+#endif
+		default:
+			break;
+	}
+}
+
+
+FSM_STATE(wait_for_deposit) 
+{
+	/* start event ocurred */
+	switch (FSM_EVENT) {
+		case EV_ENTRY:
+			FSM_TIMER(2000);
+			break;
+		case EV_START:
+			// If start is given in two seconds
+			SUBFSM_TRANSITION(deposite_balls, NULL);
+			break;
+		case EV_RETURN:
+			FSM_TRANSITION(wait_for_deposit);
 			break;
 		case EV_TIMER:
-// 			FSM_TRANSITION(go_to_container);
-// 			FSM_TRANSITION(get_balls);
+			FSM_TRANSITION(wait_for_start);
+			break;
+		default:
 			break;
- 		default: break;
 	}
 }
 
@@ -524,11 +553,14 @@ FSM_STATE(go_back_to_edge)
 		case EV_MOTION_DONE:
 			if (closed_to_container()) {
 				robot.carousel_pos = 0;
-				FSM_TRANSITION(deposite_balls);
+				SUBFSM_TRANSITION(deposite_balls, NULL);
 			} else {
 				DBG("FIXME: go_closer_to_container\n");
 			}
 			break;
+		case EV_RETURN:
+			FSM_TRANSITION(decide_where_now);
+			break;
 		default: break;
 	}
 }
@@ -648,7 +680,7 @@ FSM_STATE(deposite_balls)
 				robot.carousel_cnt = 0;
 				robot.carousel_pos = 0;
 				DBG("carousel_cnt = %d\n", robot.carousel_cnt);
-				FSM_TRANSITION(decide_where_now);
+				SUBFSM_RET(NULL);
 			}
 			break;
 		case EV_RETURN:
diff --git a/src/robofsm/eb2008/robot_orte.c b/src/robofsm/eb2008/robot_orte.c
index dea456c9..c21a6b1b 100644
--- a/src/robofsm/eb2008/robot_orte.c
+++ b/src/robofsm/eb2008/robot_orte.c
@@ -181,21 +181,28 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 	switch (info->status) {
 		case NEW_DATA:
 			if (instance->start != last_instance.start) {
+				printf("state %d ", state);
+				if (instance->start)
+					printf("plugged in\n");
+				else
+					printf("plugged out\n");
+
+				
 				switch (state) {
 					case JUST_STARTED:
-						if (instance->start) {
-							FSM_SIGNAL(MAIN, EV_START_LASER, NULL);
+						if (!instance->start) {
+							FSM_SIGNAL(MAIN, EV_START, NULL);
 							state = LASER_STARTED;
 						}
 						break;
 					case LASER_STARTED:
-						if (!instance->start) {
+						if (instance->start) {
 							FSM_SIGNAL(MAIN, EV_START, NULL);
 							state = COMPETITION_STARTED;
 						}
 						break;
 					case COMPETITION_STARTED:
-						if (instance->start) {
+						if (!instance->start) {
 							robot_exit();
 						}
 						break;
-- 
2.39.2