robofsm: Handle bumper (obstacle behind) by stopping

diff --git a/src/robofsm/fsmmove.cc b/src/robofsm/fsmmove.cc
index bd2d6be5da54430363cce6d97e7d67e2b041f33a..8c4f552ae52f2d0ae022350a437dd4a6fd2ed2aa 100644 (file)
--- a/src/robofsm/fsmmove.cc
+++ b/src/robofsm/fsmmove.cc
@@ -306,7 +306,8 @@ FSM_STATE(movement)
                        }
                        break;
                case EV_OBSTACLE_BEHIND:
-                       FSM_TRANSITION(wait_and_try_again);
+                       if (robot.moves_backward && robot.use_back_switch)
+                               FSM_TRANSITION(wait_and_try_again);
                        break;
                case EV_TRAJECTORY_LOST:
                        FSM_TRANSITION(lost);
@@ -335,7 +336,8 @@ FSM_STATE(close_to_target)
                        FSM_TRANSITION(wait_for_command);
                        break;
                case EV_OBSTACLE_BEHIND:
-                       FSM_TRANSITION(wait_and_try_again);
+                       if (robot.moves_backward && robot.use_back_switch)
+                               FSM_TRANSITION(wait_and_try_again);
                        break;
                case EV_TRAJECTORY_LOST:
                case EV_TIMER:
@@ -416,7 +418,6 @@ FSM_STATE(wait_and_try_again)
                        FSM_TRANSITION(wait_for_command);
                        break;
                case EV_OBSTACLE_BEHIND:
-                       FSM_TRANSITION(wait_and_try_again);
                        break;
                case EV_NEW_TARGET:
                case EV_TRAJECTORY_LOST:

diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index 80a4984bda5aeebbc0d2e4eb0bc7203c3026ed1d..8ecad2fde65ed42356dbcbf65c8e45105da87060 100644 (file)
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -141,8 +141,7 @@ struct robot {
        pthread_mutex_t lock_camera;
 
        /* competition parameters */
-#define team_color     orte.camera_control.game_color
-//        enum team_color team_color;
+        enum team_color team_color;
 
        /** State variable used for controlling the robot by pluggin
         * in and out start connector. */

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 08c65651f22d2fac822f4316c8e10c1cada3baed..8c864cfb87cf19e1206446294931f367c4bb620c 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -371,6 +371,21 @@ void rcv_vidle_status_cb(const ORTERecvInfo *info, void *vinstance,
        }
 }
 
+void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance,
+                          void *recvCallBackParam)
+{
+       struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
+       switch (info->status) {
+               case NEW_DATA:
+                       robot.team_color = instance->team_color;
+                       if (instance->bumper_pressed)
+                               FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
+                       break;
+               case DEADLINE:
+                       break;
+       }
+}
+
 #define HIST_CNT 5
 #if 0
 static int cmp_double(const void *v1, const void *v2)
@@ -424,6 +439,7 @@ int robot_init_orte()
        robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
        robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
        robottype_subscriber_vidle_status_create(&robot.orte, rcv_vidle_status_cb, &robot.orte);
+       robottype_subscriber_robot_switches_create(&robot.orte, rcv_robot_switches_cb, &robot.orte);
        robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);      
 
        return rv;