robofsm: FSM state survey tuning.

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 96c8925a17577986ca1f30bdcb1c2b77b4be93b7..04ab9f84035be024e358f22dc0d18940f2a3f680 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -166,7 +166,17 @@ double get_approach_angle(double xtarget, double ytarget)
 
         return atan2((ytarget - yrobot), (xtarget - xrobot));
 }
-/* automaton to survey neighborhood in two stages */
+
+
+/**
+ * FSM state for neighborhood observation.
+ *
+ * Detect targets using shape_detect.
+ * If no target detected, turn 120deg and try again.
+ * Scan all 360deg and then go back to move_around state.
+ *
+ * If target detected, go to approach_target state.
+ */
 FSM_STATE(survey)
 {
         static char turn_cntr = 0;
@@ -174,28 +184,30 @@ FSM_STATE(survey)
 
         switch(FSM_EVENT) {
                 case EV_ENTRY:
-                        /* run shape detection
-                        - if detection is positive go to the target
-                        - if no target detected turn 120° to the left */
-                        if (detect_target() & 0) {
+#if 0   // FIXME just for test
+                        if (detect_target()) {
+#else
+                        if (turn_cntr > 1) {
+                                robot_pos_type target;
+                                detected_target.clear();
+                                for (double i = 1; i < 5; i++) {
+                                        target.x = i;
+                                        target.y = i/2.0;
+                                        detected_target.push_back(target);
+                                }
+#endif
                                 // target detected, go to the target
-                                robot.target_detected = true;
                                 FSM_TRANSITION(approach_target);
                                 DBG_PRINT_EVENT("Target detected!");
                         } else {
                                 // no target detected in this heading, turn 120°
-                                robot.target_detected = false;
                                 robot_get_est_pos(&x, &y, &phi);
                                 robot_goto_notrans(x, y, TURN(DEG2RAD(120)+phi), &tcSlow);
                                 turn_cntr++;
                         }
                         break;
-                case EV_CRANE_DONE:
-                        //FSM_TIMER(1000);
-                        break;
                 case EV_TIMER:
                         if (turn_cntr > 2) {
-                                turn_cntr = 0;
                                 FSM_TRANSITION(move_around);
                         } else {
                                 FSM_TRANSITION(survey);
@@ -206,13 +218,15 @@ FSM_STATE(survey)
                         break;
                 case EV_START:
                 case EV_RETURN:
+                case EV_CRANE_DONE:
+                case EV_SWITCH_STRATEGY:
+                        DBG_PRINT_EVENT("unhandled event!");
                         break;
                 case EV_MOTION_ERROR:
-                case EV_SWITCH_STRATEGY:
-                        DBG_PRINT_EVENT("unhandled event");
                         FSM_TRANSITION(move_around);
                         break;
                 case EV_EXIT:
+                        turn_cntr = 0;
                         break;
         }
 }