robot_orte: actuator_status changed to generate EV_LIFT_IN_POS and EV_PUSHER_IN_POS...

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 026531197a73ef6481b7fa96b783063637d37584..4e13566cf9aadea23da265643713fe175cd15363 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -247,18 +247,24 @@ void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 void rcv_actuator_status_cb(const ORTERecvInfo *info, void *vinstance,
                            void *recvCallBackParam)
 {
+       static unsigned short old_lift_pos=0;
+       static unsigned short old_pusher_pos=0;
        switch (info->status) {
                case NEW_DATA:
-                       // requested position equals actual position...
-                       if (robot.orte.actuator_status.lift_pos == robot.orte.lift.pos)
+                       // new data arrived and requested position equals actual position
+                       if (robot.orte.actuator_status.lift_pos == robot.orte.lift.pos &&
+                                               robot.orte.actuator_status.lift_pos != old_lift_pos)
                                FSM_SIGNAL(ACT, EV_LIFT_IN_POS, NULL);
-                       if (robot.orte.actuator_status.pusher_pos == robot.orte.pusher.pos)
+                       if (robot.orte.actuator_status.pusher_pos == robot.orte.pusher.pos &&
+                                               robot.orte.actuator_status.pusher_pos != old_pusher_pos)
                                FSM_SIGNAL(ACT, EV_PUSHER_IN_POS, NULL);
                        if (robot.orte.actuator_status.status == 0) {
                                robot.hw_status[STATUS_LFT]=HW_STATUS_OK;
                        } else {
                                robot.hw_status[STATUS_LFT]=HW_STATUS_WARNING;
                        }
+                       old_lift_pos   = robot.orte.actuator_status.lift_pos;
+                       old_pusher_pos = robot.orte.actuator_status.pusher_pos;
                        break;
                case DEADLINE:
                        robot.hw_status[STATUS_LFT]=HW_STATUS_FAILED;