actlib, robottypes.{idl,ortegen}, can_ids.h: hokuyo pitch added

[eurobot/public.git] / src / cand / cand.cc

diff --git a/src/cand/cand.cc b/src/cand/cand.cc
index 25ffea41ecb13fce9eba63331f888fc5c68adac3..4b6a8fe15d267302e02da3c440baa8f18e6cbbbc 100644 (file)
--- a/src/cand/cand.cc
+++ b/src/cand/cand.cc
@@ -208,6 +208,20 @@ int set_pusher(struct robottype_orte_data *orte_data)
        return 0;
 }
 
+/**
+ * Sends #CAN_HOKUYO_PITCH message.
+ * 
+ * - data[1] = orte_data->pusher.pos & 0xFF; // lower byte
+ * - data[0] = orte_data->pusher.pos >> 8; // upper byte
+ */
+int set_hokuyo_pitch(struct robottype_orte_data *orte_data)
+{
+       unsigned char data = orte_data->hokuyo_pitch.angle;
+
+       can_send(CAN_HOKUYO_PITCH, sizeof(data), &data);
+       return 0;
+}
+
 
 int can_send(canid_t id, unsigned char length, unsigned char *data)
 {
@@ -473,6 +487,22 @@ void rcv_pusher_cb (const ORTERecvInfo *info, void *vinstance,
        }
 }
 
+void rcv_hokuyo_pitch_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_hokuyo_pitch(orte_data);    
+                       break;
+               case DEADLINE:
+//                     printf("ORTE deadline occurred - hokuyo pitch receive\n");
+                       break;
+       }
+}
+
+
 int main(int argc, char *argv[])
 {
        fd_set read_fd_set;
@@ -522,6 +552,7 @@ int main(int argc, char *argv[])
        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);
+       robottype_subscriber_hokuyo_pitch_create(&orte, rcv_hokuyo_pitch_cb, &orte);
 
        printf("subscribers OK\n");