cand: added actuators handling

diff --git a/src/cand/cand.cc b/src/cand/cand.cc
index 5c67085208f6a07003020ab067f0feebde541fdc..9066d7b40e1a6b17de1068fde1c2dc0671a0715c 100644 (file)
--- a/src/cand/cand.cc
+++ b/src/cand/cand.cc
@@ -96,21 +96,6 @@ int set_pwr_alert(struct robottype_orte_data *orte_data)
        return 0;
 }
 
-int set_picker(struct robottype_orte_data *orte_data)
-{
-       unsigned char data[5];
-
-       data[0] = orte_data->picker.servo_left;
-       data[1] = orte_data->picker.servo_right;
-       data[2] = orte_data->picker.drive_left;
-       data[3] = orte_data->picker.drive_right;
-       data[4] = orte_data->picker.time;
-
-       can_send(CAN_PICKER, 5, data);
-
-       return 0;
-}
-
 int send_can_msg(struct robottype_orte_data *orte_data)
 {
        uint8_t data[8];
@@ -146,39 +131,90 @@ int set_motor_speed(struct robottype_orte_data *orte_data)
        return 0;
 }
 
+/**
+ * Sends #CAN_LIFT message.
+ * 
+ * - data[1] = orte_data->lift.pos & 0xFF; // lower byte
+ * - data[0] = orte_data->lift.pos >> 8; // upper byte
+ */
+int set_lift(struct robottype_orte_data *orte_data)
+{
+       unsigned char data[2];
+
+       data[0] = orte_data->lift.pos >> 8;
+       data[1] = orte_data->lift.pos & 0xFF;
+       can_send(CAN_LIFT, sizeof(data), data);
+
+       return 0;
+}
+
 /**
  * Sends #CAN_SERVO message.
  * 
- * - data[3] = orte_data->servos.reserve;
- * - data[2] = orte_data->servos.holder;
- * - data[1] = orte_data->servos.brush_right;
- * - data[0] = orte_data->servos.brush_left;
+ * - data[1] = orte_data->chelas.left;
+ * - data[0] = orte_data->chelas.right;
  */
-int set_serva(struct robottype_orte_data *orte_data)
+int set_chelas(struct robottype_orte_data *orte_data)
 {
-       unsigned char data[4];
+       unsigned char data[2];
 
-       data[3] = orte_data->servos.reserve;
-       data[2] = orte_data->servos.holder;
-       data[1] = orte_data->servos.brush_right;
-       data[0] = orte_data->servos.brush_left;
-       can_send(CAN_SERVO, 4, data);
+       data[0] = orte_data->chelas.left;
+       data[1] = orte_data->chelas.right;
+       can_send(CAN_SERVO, sizeof(data), data);
 
        return 0;
 }
 
-int set_drives(struct robottype_orte_data *orte_data)
+/**
+ * Sends #CAN_DRIVES message.
+ * 
+ * - data[1] = orte_data->belts.left;
+ * - data[0] = orte_data->belts.right;
+ */
+int set_belts(struct robottype_orte_data *orte_data)
 {
-       unsigned char data[4];
+       unsigned char data[2];
+
+       data[0] = orte_data->belts.left;
+       data[1] = orte_data->belts.right;
+       can_send(CAN_DRIVES, sizeof(data), data);
+
+       return 0;
+}
+
+/**
+ * Sends #CAN_HOLDER message.
+ * 
+ * - data[0] = orte_data->holder.pos;
+ */
+int set_holder(struct robottype_orte_data *orte_data)
+{
+       unsigned char data[2];
+
+       data[0] = orte_data->holder.pos;
+       can_send(CAN_HOLDER, sizeof(data), data);
+
+       return 0;
+}
+
+/**
+ * Sends #CAN_PUSHER message.
+ * 
+ * - data[1] = orte_data->pusher.pos & 0xFF; // lower byte
+ * - data[0] = orte_data->pusher.pos >> 8; // upper byte
+ */
+int set_pusher(struct robottype_orte_data *orte_data)
+{
+       unsigned char data[2];
 
-       data[3] = orte_data->drives.pusher;
-       data[2] = orte_data->drives.lift_pos;
-       data[1] = orte_data->drives.brush_right;
-       data[0] = orte_data->drives.brush_left;
-       can_send(CAN_DRIVES, 4, data);
+       data[0] = orte_data->pusher.pos >> 8;
+       data[1] = orte_data->pusher.pos & 0xFF;
+       can_send(CAN_PUSHER, sizeof(data), data);
 
        return 0;
 }
+
+
 int can_send(canid_t id, unsigned char length, unsigned char *data)
 {
        struct can_frame frame;
@@ -373,34 +409,77 @@ void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
        }
 }
 
-void rcv_servos_cb (const ORTERecvInfo *info, void *vinstance, 
+void rcv_lift_cb (const ORTERecvInfo *info, void *vinstance, 
                        void *recvCallBackParam) 
 {
-       struct robottype_orte_data *orte_data = 
-                       (struct robottype_orte_data *)recvCallBackParam;
+       struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 
        switch (info->status) {
                case NEW_DATA:
-                       set_serva(orte_data);   
+                       set_lift(orte_data);    
                        break;
                case DEADLINE:
-//                     printf("ORTE deadline occurred - servo receive\n");
+//                     printf("ORTE deadline occurred - lift receive\n");
                        break;
        }
 }
 
+void rcv_chelas_cb (const ORTERecvInfo *info, void *vinstance, 
+                       void *recvCallBackParam) 
+{
+       struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
+
+       switch (info->status) {
+               case NEW_DATA:
+                       set_chelas(orte_data);  
+                       break;
+               case DEADLINE:
+//                     printf("ORTE deadline occurred - chelas receive\n");
+                       break;
+       }
+}
 
-void rcv_drives_cb (const ORTERecvInfo *info, void *vinstance, 
+void rcv_belts_cb (const ORTERecvInfo *info, void *vinstance, 
                        void *recvCallBackParam) 
 {
        struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 
        switch (info->status) {
                case NEW_DATA:
-                       set_drives(orte_data);  
+                       set_belts(orte_data);   
                        break;
                case DEADLINE:
-//                     printf("ORTE deadline occurred - drives receive\n");
+//                     printf("ORTE deadline occurred - belts receive\n");
+                       break;
+       }
+}
+
+void rcv_holder_cb (const ORTERecvInfo *info, void *vinstance, 
+                       void *recvCallBackParam) 
+{
+       struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
+
+       switch (info->status) {
+               case NEW_DATA:
+                       set_holder(orte_data);  
+                       break;
+               case DEADLINE:
+//                     printf("ORTE deadline occurred - holder receive\n");
+                       break;
+       }
+}
+
+void rcv_pusher_cb (const ORTERecvInfo *info, void *vinstance, 
+                       void *recvCallBackParam) 
+{
+       struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
+
+       switch (info->status) {
+               case NEW_DATA:
+                       set_pusher(orte_data);  
+                       break;
+               case DEADLINE:
+//                     printf("ORTE deadline occurred - pusher receive\n");
                        break;
        }
 }
@@ -439,19 +518,16 @@ int main(int argc, char *argv[])
        robottype_publisher_cmu_create(&orte, NULL, NULL);
 
        /* creating subscribers */
-       robottype_subscriber_pwr_ctrl_create(&orte, 
-                               rcv_pwr_ctrl_cb, &orte);
-       robottype_subscriber_pwr_alert_create(&orte, 
-                               rcv_pwr_alert_cb, &orte);
-       robottype_subscriber_motion_speed_create(&orte, 
-                               rcv_motion_speed_cb, &orte);
-       robottype_subscriber_can_msg_create(&orte, 
-                               rcv_can_msg_cb, &orte);
-       robottype_subscriber_servos_create(&orte, 
-                               rcv_servos_cb, &orte);
-
-       robottype_subscriber_drives_create(&orte, 
-                               rcv_drives_cb, &orte);
+       robottype_subscriber_pwr_ctrl_create(&orte, rcv_pwr_ctrl_cb, &orte);
+       robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
+       robottype_subscriber_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
+       robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
+       robottype_subscriber_lift_create(&orte, rcv_lift_cb, &orte);
+       robottype_subscriber_chelas_create(&orte, rcv_lift_cb, &orte);
+       robottype_subscriber_belts_create(&orte, rcv_belts_cb, &orte);
+       robottype_subscriber_holder_create(&orte, rcv_holder_cb, &orte);
+       robottype_subscriber_pusher_create(&orte, rcv_pusher_cb, &orte);
+
        /* main loop */
        for(;;) {
                FD_ZERO(&read_fd_set);