robot_orte.c: puck presence switch handling added

[eurobot/public.git] / src / robofsm / robot_orte.c

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index f75b083487c62a55d351883a8c3003363948d996..d54c1c422d45457c4553aa867fec4ebee3fd1100 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -311,6 +311,23 @@ void rcv_puck_distance_cb(const ORTERecvInfo *info, void *vinstance,
        }
 }
 
+void rcv_puck_inside_cb(const ORTERecvInfo *info, void *vinstance,
+                           void *recvCallBackParam)
+{
+       static bool previous_switch_status;
+       switch (info->status) {
+       case NEW_DATA: {
+                       if (robot.orte.puck_inside.status == 1 && previous_switch_status != 1)
+                               FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
+                       previous_switch_status = robot.orte.puck_inside.status;
+               }
+               break;
+       case DEADLINE:
+               DBG("ORTE deadline occurred - servos receive\n");
+               break;
+       }
+}
+
 #define HIST_CNT 5
 #if 0
 static int cmp_double(const void *v1, const void *v2)
@@ -368,7 +385,8 @@ int robot_init_orte()
 
        /* create subscribers */
        robottype_subscriber_actuator_status_create(&robot.orte, rcv_actuator_status_cb, &robot.orte);
-       robottype_subscriber_puck_distance_create(&robot.orte, rcv_puck_distance_cb, &robot.orte);
+       //robottype_subscriber_puck_distance_create(&robot.orte, rcv_puck_distance_cb, &robot.orte);
+       robottype_subscriber_puck_inside_create(&robot.orte, rcv_puck_inside_cb, &robot.orte);
        robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
 
        return rv;