robofsm: Add lift and jaws acuators to orte.

Added receive lift and jaws callback function.

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 19f296e417227a6687e2cbab6fd5dc475437f036..6ccf605ba5e6329ea3d8aaf087e3503be127dfcb 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -349,31 +349,62 @@ void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
  * SUBSCRIBER CALLBACKS - EB2008
  * ---------------------------------------------------------------------- */
 
-void rcv_vidle_status_cb(const ORTERecvInfo *info, void *vinstance,
+void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance,
                            void *recvCallBackParam)
 {
-       struct vidle_status_type *instance = (struct vidle_status_type *)vinstance;
-       static int last_response = 0;
+       struct jaws_status_type *instance = (struct jaws_status_type *)vinstance;
+       static int last_response_left = 0;
+       static int last_response_right = 0;
        switch (info->status) {
                case NEW_DATA:
                        // new data arrived and requested position equals actual position
-                       if (instance->flags == 0)
-                               robot.status[COMPONENT_VIDLE]=STATUS_OK;
+                       if (instance->flags.left == 0 &&
+                            instance->flags.right == 0)
+                               robot.status[COMPONENT_JAWS] = STATUS_OK;
                        else
-                               robot.status[COMPONENT_VIDLE]=STATUS_WARNING;
+                               robot.status[COMPONENT_JAWS] = STATUS_WARNING;
 
-                       if (instance->response != last_response &&
-                           instance->response == act_vidle_get_last_reqest())
-                               FSM_SIGNAL(MAIN, EV_VIDLE_DONE, NULL);
-                       last_response = instance->response;
+                       if (instance->response.left != last_response_left &&
+                            instance->response.right != last_response_right &&
+                           instance->response.left == act_jaw_left_get_last_reqest() &&
+                           instance->response.left == act_jaw_right_get_last_reqest())
+                               FSM_SIGNAL(MAIN, EV_JAWS_DONE, NULL);
+
+                       last_response_left = instance->response.left;
+                       last_response_right = instance->response.right;
                        break;
                case DEADLINE:
-                       robot.status[COMPONENT_VIDLE]=STATUS_FAILED;
+                       robot.status[COMPONENT_JAWS] = STATUS_FAILED;
                        DBG("ORTE deadline occurred - actuator_status receive\n");
                        break;
        }
 }
 
+void rcv_lift_status_cb(const ORTERecvInfo *info, void *vinstance,
+                            void *recvCallBackParam)
+{
+        struct lift_status_type *instance = (struct lift_status_type *)vinstance;
+        static int last_response = 0;
+        switch (info->status) {
+                case NEW_DATA:
+                        // new data arrived and requested position equals actual position
+                        if (instance->flags == 0)
+                                robot.status[COMPONENT_LIFT] = STATUS_OK;
+                        else
+                                robot.status[COMPONENT_LIFT] = STATUS_WARNING;
+
+                        if (instance->response != last_response &&
+                            instance->response == act_lift_get_last_reqest())
+                                FSM_SIGNAL(MAIN, EV_LIFT_DONE, NULL);
+                        last_response = instance->response;
+                        break;
+                case DEADLINE:
+                        robot.status[COMPONENT_LIFT] = STATUS_FAILED;
+                        DBG("ORTE deadline occurred - actuator_status receive\n");
+                        break;
+        }
+}
+
 void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance,
                           void *recvCallBackParam)
 {
@@ -441,7 +472,8 @@ int robot_init_orte()
        robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte);
        robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte);
        robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
-       robottype_publisher_vidle_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
+       robottype_publisher_jaws_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
+        robottype_publisher_lift_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
 
        /* create generic subscribers */
        robottype_subscriber_odo_data_create(&robot.orte, rcv_odo_data_cb, &robot.orte);
@@ -452,9 +484,10 @@ int robot_init_orte()
        //robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.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_jaws_status_create(&robot.orte, rcv_jaws_status_cb, &robot.orte);
+        robottype_subscriber_lift_status_create(&robot.orte, rcv_lift_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);      
+       robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
 
        return rv;
 }