cand: CAN_LP_STATUS handling added

diff --git a/src/cand/cand.cc b/src/cand/cand.cc
index 84395ccb621bc26ab70bba575cbdbfe51c0b0ba0..5367490585058a6d746bb55ea3d5941fdd8e680f 100644 (file)
--- a/src/cand/cand.cc
+++ b/src/cand/cand.cc
@@ -325,12 +325,19 @@ int cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
                        ORTEPublicationSend(orte->publication_pwr_voltage);
                        
                        break;
+               /* data from the sharp sensor measuring puck to sensor distance */
                case CAN_ADC_1: /* data from the sharp sensor measuring distance of the puck (column element) */
-                       //orte->puck_distance.distance = ((frame.data[0]<<8)|(frame.data[1]));
-                       orte->puck_distance.distance = puckSharp_ir2mm((frame.data[0]<<8)|(frame.data[1]))/1000.0;
+                       orte->puck_distance.distance = puckSharp_ir2mm(frame.data[0]<<8 | frame.data[1])/1000.0;
                        ORTEPublicationSend(orte->publication_puck_distance);
                        break;
-
+               /* lift & pusher status, their position and status bit */
+               case CAN_LP_STATUS:
+                       /* printf("lift position: %d \n", frame.data[1]<<8 | frame.data[2]); */
+                       orte->actuator_status.pusher_pos = frame.data[0]<<8 | frame.data[1];
+                       orte->actuator_status.lift_pos   = frame.data[2]<<8 | frame.data[3];
+                       orte->actuator_status.status     = frame.data[4];
+                       ORTEPublicationSend(orte->publication_actuator_status);
+                       break;
                /* laser data */
                case CAN_CMU:
                        orte->cmu.color = frame.data[0];
@@ -537,6 +544,7 @@ int main(int argc, char *argv[])
        robottype_publisher_corr_trig_create(&orte, NULL, NULL);
        robottype_publisher_corr_distances_create(&orte, NULL, NULL);
        robottype_publisher_puck_distance_create(&orte, NULL, NULL);
+       robottype_publisher_actuator_status_create(&orte, NULL, NULL);
        printf("Publishers OK\n");
 
        /* creating subscribers */