robofsm: Strategy automaton tunning.

- remove unused events
- debug messages update

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index d401f5e8ae6edc055ea80d131a63cfd146d458b9..46267c57ca03999132b568b74be43fe70721bdbc 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -213,12 +213,6 @@ FSM_STATE(survey)
                 case EV_MOTION_DONE:
                         FSM_TIMER(1000);
                         break;
-                case EV_START:
-                case EV_RETURN:
-                case EV_JAWS_DONE:
-                case EV_SWITCH_STRATEGY:
-                        DBG_PRINT_EVENT("unhandled event!");
-                        break;
                 case EV_MOTION_ERROR:
                         FSM_TRANSITION(move_around);
                         break;
@@ -252,6 +246,8 @@ FSM_STATE(approach_target)
                         x_target = detected_target[target_cntr].x;
                         y_tatget = detected_target[target_cntr].y;
                         target_cntr++;
+                        
+                        printf("target %d / %d\n", target_cntr, max_target);
 
                         get_approach_point(x_target, y_tatget, &x_approach, &y_approach, &phi_approach);
                         robot_goto_notrans(x_approach, y_approach, ARRIVE_FROM(phi_approach, 0.2), &tcSlow);
@@ -275,19 +271,13 @@ FSM_STATE(approach_target)
                         }
                         break;
                 case EV_MOTION_ERROR:
-                        DBG_PRINT_EVENT("ERROR approaching");
+                        DBG_PRINT_EVENT("can not approach target");
                         if (target_cntr < max_target) {
                                 FSM_TRANSITION(approach_target);
                         } else {
                                 FSM_TRANSITION(move_around);
                         }
                         break;
-                case EV_TIMER:
-                case EV_START:
-                case EV_JAWS_DONE:
-                case EV_SWITCH_STRATEGY:
-                        DBG_PRINT_EVENT("unhandled event");
-                        break;
                 case EV_EXIT:
                         target_cntr = 0;
                         break;
@@ -296,37 +286,26 @@ FSM_STATE(approach_target)
 
 FSM_STATE(move_around)
 {
-        double goalx, goaly, phi;
-        static int survey_cnt = 0;
+        double goalx, goaly;
 
         switch (FSM_EVENT) {
                 case EV_ENTRY:
-
                         do {
                                 goalx = ((rand()%PLAYGROUND_HEIGHT_MM)/1000.0);
-                                goaly = ((rand()%PLAYGROUND_WIDTH_MM)/1000.0);
+                                goaly += ((rand()%PLAYGROUND_WIDTH_MM)/1000.0);
                         } while (!ShmapIsFreePoint(goalx, goaly) && close_goal(goalx, goaly));
 
-                        robot_goto_notrans(goalx, goaly, NO_TURN(), &tcFast);
+                        robot_goto_notrans(goalx, goaly, NO_TURN(), &tcSlow);
                         DBG_PRINT_EVENT("new survey point");
-                        survey_cnt++;
                         break;
                 case EV_MOTION_ERROR:
-                        DBG_PRINT_EVENT("ERROR: can not access survey point");
+                        DBG_PRINT_EVENT("can not access survey point");
                 case EV_MOTION_DONE:
                         FSM_TRANSITION(survey);
                         break;
                 case EV_TIMER:
                         break;
-                case EV_RETURN:
-                        break;
-                case EV_START:
-                        /* do nothing */
-                        break;
                 case EV_EXIT:
-                        //ShmapFree();
-                        break;
-                default:
                         break;
         }
 }
@@ -342,17 +321,11 @@ FSM_STATE(go_home)
                         DBG_PRINT_EVENT("ERROR: home position is not reachable!");
                         FSM_TIMER(1000);
                         break;
-                case EV_MOTION_DONE:
-                        break;
                 case EV_TIMER:
                         FSM_TRANSITION(go_home);
                         break;
-                case EV_START:
-                        /* do nothing */
-                        break;
+                case EV_MOTION_DONE:
                 case EV_EXIT:
-                        break;
-                default:
                         DBG_PRINT_EVENT("Mission completed!");
                         robot_exit();
                         break;