From: Michal Sojka <sojkam1@fel.cvut.cz>
Date: Fri, 24 Apr 2009 13:20:15 +0000 (+0200)
Subject: robofsm: Back switches handling
X-Git-Tag: eb2009~29
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/54a60f53c70e8d965a0bc96c4e980572ec9aa495

robofsm: Back switches handling
---

diff --git a/src/robofsm/fsmmove.cc b/src/robofsm/fsmmove.cc
index 936d175c..74b3e48c 100644
--- a/src/robofsm/fsmmove.cc
+++ b/src/robofsm/fsmmove.cc
@@ -262,6 +262,9 @@ FSM_STATE(movement)
 				case TARGET_ERROR: FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL); printf("Target error\n"); break;
 			}
 			break;
+		case EV_OBSTACLE_BEHIND:
+			FSM_TRANSITION(wait_and_try_again);
+			break;
 		case EV_TRAJECTORY_LOST:
 			FSM_TRANSITION(lost);
 			break;
@@ -289,6 +292,9 @@ FSM_STATE(close_to_target)
 			FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
 			FSM_TRANSITION(wait_for_command);
 			break;
+		case EV_OBSTACLE_BEHIND:
+			FSM_TRANSITION(wait_and_try_again);
+			break;
 		case EV_TRAJECTORY_LOST:
 		case EV_TIMER:
 			FSM_TRANSITION(lost);
@@ -327,6 +333,7 @@ FSM_STATE(lost)
 			break;
 		case EV_EXIT:
 			break;
+		case EV_OBSTACLE_BEHIND:
 		case EV_TRAJECTORY_DONE:
 		case EV_NEW_TARGET:
 		case EV_TRAJECTORY_DONE_AND_CLOSE:
@@ -359,6 +366,8 @@ FSM_STATE(wait_and_try_again)
 		case EV_MOVE_STOP:
 			FSM_TRANSITION(wait_for_command);
 			break;
+		case EV_OBSTACLE_BEHIND:
+			break;
 		case EV_NEW_TARGET:
 		case EV_TRAJECTORY_LOST:
 		case EV_RETURN:
diff --git a/src/robofsm/roboevent.py b/src/robofsm/roboevent.py
index 8771645d..20ba59af 100644
--- a/src/robofsm/roboevent.py
+++ b/src/robofsm/roboevent.py
@@ -28,6 +28,7 @@ events = {
         "EV_TRAJECTORY_DONE" : "Reference position is at the end of the previously submitted trajectory",
 	"EV_TRAJECTORY_DONE_AND_CLOSE" : "::EV_TRAJECTORY_DONE + distance of actual position to the target is less than XXX",
 	"EV_OBSTACLE" : "An obstacle is detected on actual trajectory - we must avoid it.",
+	"EV_OBSTACLE_BEHIND" : "",
 	#"EV_FOUND_AFTER_RESET" : "MCL estimated position after reset",
 	#"EV_RECALCULATION_FAILED" : "Trajectory recalculation caused by a detected obstcale has failed (error or target not reachable)",
     },
diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 50248dd4..cc98ee41 100644
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -320,8 +320,10 @@ void rcv_puck_inside_cb(const ORTERecvInfo *info, void *vinstance,
 	static bool previous_switch_status;
 	switch (info->status) {
 	case NEW_DATA: {
-			if (robot.orte.puck_inside.status == 1 && previous_switch_status != 1)
-				FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
+		if (robot.orte.puck_inside.status == 1 && previous_switch_status != 1) {
+			//FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
+			FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
+		}
 			previous_switch_status = robot.orte.puck_inside.status;
 			robot.hw_status[STATUS_CHE]=HW_STATUS_OK;
 		}