From 8a6d94d23f982d88c0f431b283202c2ebd59fa66 Mon Sep 17 00:00:00 2001 From: Michal Vokac Date: Wed, 11 Jan 2012 21:03:29 +0100 Subject: [PATCH] robofsm: Add bumpers subscriber and fix bumpers handling. --- src/robofsm/robot_orte.c | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c index 24381708..bc952c4f 100644 --- a/src/robofsm/robot_orte.c +++ b/src/robofsm/robot_orte.c @@ -442,17 +442,15 @@ void rcv_robot_bumpers_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) { struct robot_bumpers_type *instance = (struct robot_bumpers_type *)vinstance; - static bool last_left, last_right; switch (info->status) { case NEW_DATA: - if (instance->bumper_rear) + if (!instance->bumper_rear) { FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL); - if ((!last_left && instance->bumper_left) || - (!last_right && instance->bumper_right)) { + } + if ((!instance->bumper_left) || + (!instance->bumper_right)) { FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL); } - last_right = instance->bumper_right; - last_left = instance->bumper_left; break; case DEADLINE: break; @@ -516,6 +514,7 @@ int robot_init_orte() robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte); robottype_subscriber_crane_status_create(&robot.orte, rcv_crane_status_cb, &robot.orte); robottype_subscriber_robot_switches_create(&robot.orte, rcv_robot_switches_cb, &robot.orte); + robottype_subscriber_robot_bumpers_create(&robot.orte, rcv_robot_bumpers_cb, &robot.orte); robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte); return rv; -- 2.39.2