Merge branch 'sick-day' into update

diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc
index 12a3db4fa0e575ab796342a45f6bde12eb2bc196..d401f5e8ae6edc055ea80d131a63cfd146d458b9 100644 (file)
--- a/src/robofsm/common-states.cc
+++ b/src/robofsm/common-states.cc
@@ -125,10 +125,8 @@ static bool detect_target()
                         if (goal_is_in_playground(target.x, target.y))
                                 detected_target.push_back(target);
                 }
-                return true;
-        } else {
-                return false;
         }
+        return detected_target.size();
 }
 
 /**
@@ -185,6 +183,7 @@ FSM_STATE(survey)
 
         switch(FSM_EVENT) {
                 case EV_ENTRY:
+                        DBG_PRINT_EVENT("survey");
                         if (turn_cntr > 1) {
                                 robot_pos_type target;
                                 detected_target.clear();
@@ -201,6 +200,7 @@ FSM_STATE(survey)
                                 robot_get_est_pos(&x, &y, &phi);
                                 robot_goto_notrans(x, y, TURN(DEG2RAD(120)+phi), &tcSlow);
                                 turn_cntr++;
+                                DBG_PRINT_EVENT("no target");
                         }
                         break;
                 case EV_TIMER:
@@ -248,28 +248,34 @@ FSM_STATE(approach_target)
 
         switch(FSM_EVENT) {
                 case EV_ENTRY:
+                        DBG_PRINT_EVENT("approaching target");
                         x_target = detected_target[target_cntr].x;
                         y_tatget = detected_target[target_cntr].y;
                         target_cntr++;
 
                         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);
-
-                        printf("regular target x:%f y:%f\n",x_target,y_tatget);
                         break;
                 case EV_MOTION_DONE:
+                        DBG_PRINT_EVENT("target approached");
                         SUBFSM_TRANSITION(recognize, NULL);
                         break;
                 case EV_RETURN:
                         if (robot.target_loaded) {
                                 FSM_TRANSITION(go_home);
+                        } else if (robot.target_valid) {
+                                //FIXME target is valid but not loaded - try another approach direction
+
                         } else if (!robot.target_valid && (target_cntr < max_target)) {
+                                // go for next target if any
                                 FSM_TRANSITION(approach_target);
                         } else {
+                                // go to new point and survey
                                 FSM_TRANSITION(move_around);
                         }
                         break;
                 case EV_MOTION_ERROR:
+                        DBG_PRINT_EVENT("ERROR approaching");
                         if (target_cntr < max_target) {
                                 FSM_TRANSITION(approach_target);
                         } else {
@@ -295,28 +301,22 @@ FSM_STATE(move_around)
 
         switch (FSM_EVENT) {
                 case EV_ENTRY:
-                        /* TODO upravit generovani nahodne pozice tak aby se lepe filtrovaly
-                         uz prozkomane body a prekazky i body mimo hriste.
-                         Oznacit v mape vsechny oblasti prozkoumane pomoci hokuya na kterych nedoslo k detekci??
-                         */
 
-                        do {;
+                        do {
                                 goalx = ((rand()%PLAYGROUND_HEIGHT_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);
+                        DBG_PRINT_EVENT("new survey point");
                         survey_cnt++;
                         break;
                 case EV_MOTION_ERROR:
-                        ul_logdeb("Goal is not reachable\n");
-                        FSM_TIMER(1000);
-                        break;
+                        DBG_PRINT_EVENT("ERROR: can not access survey point");
                 case EV_MOTION_DONE:
                         FSM_TRANSITION(survey);
                         break;
                 case EV_TIMER:
-                        FSM_TRANSITION(move_around);
                         break;
                 case EV_RETURN:
                         break;
@@ -335,10 +335,11 @@ FSM_STATE(go_home)
 {
         switch (FSM_EVENT) {
                 case EV_ENTRY:
-                        robot_goto_notrans(HOME_POS_X_M, HOME_POS_Y_M, ARRIVE_FROM(0, 0.5), &tcFast);
+                        DBG_PRINT_EVENT("homing");
+                        robot_goto_notrans(HOME_POS_X_M, HOME_POS_Y_M, ARRIVE_FROM(DEG2RAD(HOME_POS_ANG_DEG), 0.2), &tcSlow);
                         break;
                 case EV_MOTION_ERROR:
-                        ul_logdeb("Goal is not reachable\n");
+                        DBG_PRINT_EVENT("ERROR: home position is not reachable!");
                         FSM_TIMER(1000);
                         break;
                 case EV_MOTION_DONE:
@@ -350,9 +351,10 @@ FSM_STATE(go_home)
                         /* do nothing */
                         break;
                 case EV_EXIT:
-                        //ShmapFree();
                         break;
                 default:
+                        DBG_PRINT_EVENT("Mission completed!");
+                        robot_exit();
                         break;
         }
 }

diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c
index 3f615164db7e761d276c3c18154b33b68c1c6df3..816a747477ab36c6b774f54ca2e4dad6c82f8d70 100644 (file)
--- a/src/robofsm/robot.c
+++ b/src/robofsm/robot.c
@@ -144,7 +144,7 @@ int robot_init()
        robot.orte.pwr_ctrl.voltage50 = 1;
        robot.orte.pwr_ctrl.voltage80 = 1;
 
-       robot.orte.camera_control.on = true;
+       robot.orte.camera_control.on = false;
 
        robot.fsm.motion.state = &fsm_state_motion_init;
 

diff --git a/src/robofsm/sub-states.cc b/src/robofsm/sub-states.cc
index a267706c616cd880a84d6ed64b41bccbd34cc8e4..912ed670326ff7c52ce5f06071ee19acf71b7086 100644 (file)
--- a/src/robofsm/sub-states.cc
+++ b/src/robofsm/sub-states.cc
@@ -55,7 +55,7 @@ void get_hokuyo_min(double *alpha, double *minimum)
 
         for (int i = 0; i < HOKUYO_ARRAY_SIZE; i++) {
                 beta = HOKUYO_INDEX_TO_RAD(i);
-                if((beta < (-70.0 / 180.0 * M_PI)) || ((beta > (70.0 / 180.0 * M_PI))))
+                if((beta < (-50.0 / 180.0 * M_PI)) || ((beta > (50.0 / 180.0 * M_PI))))
                         continue;
 
                 if(data[i] > HOK_MIN_MM && data[i] < HOK_MAX_MM) {
@@ -74,11 +74,19 @@ void get_hokuyo_min(double *alpha, double *minimum)
         printf("alpha: %f, beta: %f, minimum: %f\n", *alpha, beta, *minimum);
 }
 
+void enable_obstacle_avoidance(bool enable)
+{
+        robot.obstacle_avoidance_enabled = enable;
+        robot.ignore_hokuyo = !enable;
+        robot.check_turn_safety = enable;
+}
+
 FSM_STATE(recognize)
 {
         switch(FSM_EVENT) {
                 case EV_ENTRY:
                         act_camera_recognize(robot.target_color);
+                        DBG_PRINT_EVENT("recognition");
                         FSM_TIMER(1000);
                         break;
                 case EV_TIMER:
@@ -90,6 +98,7 @@ FSM_STATE(recognize)
                                 DBG_PRINT_EVENT("camera: Target valid!");
                                 FSM_TRANSITION(get_target_turn);
                         } else {
+                                DBG_PRINT_EVENT("camera: Target not valid!");
                                 robot.target_loaded = false;
                                 SUBFSM_RET(NULL);
                         }
@@ -106,9 +115,8 @@ FSM_STATE(get_target_turn)
 
         switch(FSM_EVENT) {
                 case EV_ENTRY:
-                        robot.obstacle_avoidance_enabled = false;
-                        robot.ignore_hokuyo = true;
-
+                        DBG_PRINT_EVENT("turn");
+                        enable_obstacle_avoidance(false);
                         get_hokuyo_min(&alpha, &minimum);
                         robot_get_est_pos(&x, &y, &phi);
                         robot_move_by(0, TURN(alpha+phi), &tcVerySlow);
@@ -119,9 +127,7 @@ FSM_STATE(get_target_turn)
                         FSM_TRANSITION(get_target_touch);
                         break;
                 case EV_MOTION_ERROR:
-                        robot.obstacle_avoidance_enabled = true;
-                        robot.ignore_hokuyo = false;
-                        robot.target_loaded = false;
+                        enable_obstacle_avoidance(true);
                         SUBFSM_RET(NULL);
                         break;
                 case EV_EXIT:
@@ -131,23 +137,29 @@ FSM_STATE(get_target_turn)
 
 FSM_STATE(get_target_touch)
 {
+        const double close = 0.05;
+        const double step = 0.02;
         double alpha, minimum;
         double x, y, phi;
 
         switch(FSM_EVENT) {
                 case EV_ENTRY:
+                        DBG_PRINT_EVENT("touch");
                         get_hokuyo_min(&alpha, &minimum);
-                        robot_move_by(minimum, NO_TURN(), &tcVerySlow);
+                        robot_move_by(step, NO_TURN(), &tcVerySlow);
                         break;
                 case EV_TIMER:
                         break;
                 case EV_MOTION_DONE:
-                        FSM_TRANSITION(get_target_load);
+                        get_hokuyo_min(&alpha, &minimum);
+                        if (minimum < close) {
+                                FSM_TRANSITION(get_target_load);
+                        } else {
+                                FSM_TRANSITION(get_target_turn);
+                        }
                         break;
                 case EV_MOTION_ERROR:
-                        robot.obstacle_avoidance_enabled = true;
-                        robot.ignore_hokuyo = false;
-                        robot.target_loaded = false;
+                        enable_obstacle_avoidance(true);
                         SUBFSM_RET(NULL);
                         break;
                 case EV_EXIT:
@@ -165,13 +177,13 @@ FSM_STATE(get_target_load)
                         act_jaws(OPEN);
                         break;
                 case EV_TIMER:
+                        act_jaws(CATCH);
                         robot.target_loaded = true;
                         FSM_TRANSITION(get_target_back);
                         break;
                 case EV_JAWS_DONE:
                         if (direction == 0) {
                                 direction = 1;
-                                act_jaws(CATCH);
                                 FSM_TIMER(2000);
                         }
                         break;
@@ -195,8 +207,7 @@ FSM_STATE(get_target_back)
                         SUBFSM_RET(NULL);
                         break;
                 case EV_EXIT:
-                        robot.obstacle_avoidance_enabled = true;
-                        robot.ignore_hokuyo = false;
+                        enable_obstacle_avoidance(true);
                         break;
         }
 }