cand: Add color sensor command subscriber

Color sensor command message is send to CAN.

diff --git a/src/cand/cand.cc b/src/cand/cand.cc
index 072216e9c1b3a0ad79e9c01b98d7e3ecb6de6f82..a3dff71ee7b6786e6970030301cb5228cae04f5c 100644 (file)
--- a/src/cand/cand.cc
+++ b/src/cand/cand.cc
@@ -371,7 +371,23 @@ void rcv_lift_cmd_cb(const ORTERecvInfo *info, void *vinstance,
                        break;
                case DEADLINE:
                        break;
+        }
 }
+
+void rcv_cl_sensor_cmd_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:
+                        unsigned char data[1];
+                        data[0] = orte_data->bank_nbr;
+                        can_send(CAN_CL_SENSOR_CMD, 1, data);
+                        break;
+                case DEADLINE:
+                        break;
+        }
 }
 
 int main(int argc, char *argv[])
@@ -418,8 +434,7 @@ int main(int argc, char *argv[])
        robottype_subscriber_jaws_cmd_create(&orte, rcv_jaws_cmd_cb, &orte);
         robottype_subscriber_lift_cmd_create(&orte, rcv_lift_cmd_cb, &orte);
        robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
-
-
+        robottype_subscriber_cl_sensor_cmd_create(&orte, rcv_cl_sensor_cmd_cb, &orte);
        printf("subscribers OK\n");
 
        /* main loop */