X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/blobdiff_plain/8b6985afd170cad11e95271773d4e8c0d252dabb..16d700e75c6c24612de49c1370eb6c293de695df:/src/cand/cand.cc

diff --git a/src/cand/cand.cc b/src/cand/cand.cc
index f45dad37..c36836da 100644
--- a/src/cand/cand.cc
+++ b/src/cand/cand.cc
@@ -19,6 +19,7 @@
 #include <sys/socket.h>
 #include <sys/ioctl.h>
 #include <fcntl.h>
+#include <poll.h>
 
 #include <af_can.h>
 #include <canutils.h>
@@ -28,7 +29,6 @@
 #include <can_msg_def.h>
 #include <sharp.h>
 #include <orte.h>
-#include <roboorte.h>
 #include <roboorte_robottype.h>
 #include <can_msg_masks.h>
 #include "cand.h"
@@ -86,30 +86,9 @@ int set_pwr_ctrl(struct robottype_orte_data *orte_data)
 	return 0;
 }
 
-int set_pwr_alert(struct robottype_orte_data *orte_data)
-{
-	unsigned char data=0;
-	data = orte_data->pwr_alert.value;
-
-	can_send(CAN_PWR_ALERT, 1, &data);
-
-	return 0;
-}
-
 int send_can_msg(struct robottype_orte_data *orte_data)
 {
-	uint8_t data[8];
-
-	data[0] = orte_data->can_msg.data1;
-	data[1] = orte_data->can_msg.data2;
-	data[2] = orte_data->can_msg.data3;
-	data[3] = orte_data->can_msg.data4;
-	data[4] = orte_data->can_msg.data5;
-	data[5] = orte_data->can_msg.data6;
-	data[6] = orte_data->can_msg.data7;
-	data[7] = orte_data->can_msg.data8;
-
-	can_send(orte_data->can_msg.id, orte_data->can_msg.len, data);
+	return can_send(orte_data->can_msg.id, orte_data->can_msg.len, orte_data->can_msg.data);
 }
 
 int set_motor_speed(struct robottype_orte_data *orte_data)
@@ -122,62 +101,43 @@ int set_motor_speed(struct robottype_orte_data *orte_data)
 	data[3] = orte_data->motion_speed.left & 0xff;
 	can_send(CAN_MOTION_CMD, 4, data);
 
-	/*printf("data: ");
-	for (int i=0; i< 4; i++) {
-		printf("%02x ",data[i]);
-	}
-	printf("\n");*/
-
 	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;
- */
-int set_serva(struct robottype_orte_data *orte_data)
+int set_jaws_cmd(struct robottype_orte_data *orte_data)
 {
-	unsigned char data[4];
-
-	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);
-
-	return 0;
-}
+	unsigned char data[3];
 
-int set_laser_cmd(struct robottype_orte_data *orte_data)
-{
-	unsigned char data[1];
+	data[0] = orte_data->jaws_cmd.req_pos.left >> 8;
+	data[1] = orte_data->jaws_cmd.req_pos.left & 0xff;
+	data[2] = orte_data->jaws_cmd.speed.left;
+	can_send(CAN_JAW_LEFT_CMD, 3, data);
 
-	data[0] = orte_data->laser_cmd.speed;
-	can_send(CAN_LAS_CMD, 1, data);
+	data[0] = orte_data->jaws_cmd.req_pos.right >> 8;
+	data[1] = orte_data->jaws_cmd.req_pos.right & 0xff;
+	data[2] = orte_data->jaws_cmd.speed.right;
+	can_send(CAN_JAW_RIGHT_CMD, 3, data);
 
 	return 0;
 }
 
-int set_drives(struct robottype_orte_data *orte_data)
+int set_lift_cmd(struct robottype_orte_data *orte_data)
 {
 	unsigned char data[4];
 
-	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->lift_cmd.req_pos >> 8;
+	data[1] = orte_data->lift_cmd.req_pos & 0xff;
+	data[2] = orte_data->lift_cmd.speed;
+        data[3] = orte_data->lift_cmd.homing;
+	can_send(CAN_LIFT_CMD, 4, data);
 
-	return 0;
+        return 0;
 }
+
 int can_send(canid_t id, unsigned char length, unsigned char *data)
 {
 	struct can_frame frame;
-	int size;
+	size_t size;
 
 	frame.can_id = id;
 	frame.can_dlc = length;
@@ -193,82 +153,100 @@ int can_send(canid_t id, unsigned char length, unsigned char *data)
 		return 1;
 	}
 
-	/*
-	printf("write returned %d\n", size);
-	printf("finnished can send\n");
-	*/
-
 	return 0;
 }
 
 /**
  * Parse frame ID and react as required
  */
-int cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
+void cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
 {
-	int status_cnt = 0;
-	int adc1_cnt = 0;
-	int adc2_cnt = 0;
-	int adc3_cnt = 0;
-	int ir_cnt = 0;
-	unsigned short *las_meas;
-	int las_mi, las_di, las_bcnt;
-	static unsigned short prev_meas;
-	int i;
+	static int status_cnt = 0;
 
 	switch(frame.can_id) {
-		/* voltage measurements from power board */
-		
 		/* robot commands (start, ..) */
+		case CAN_JAW_LEFT_STATUS:
+			orte->jaws_status.act_pos.left = (frame.data[0] << 8) | frame.data[1];
+			orte->jaws_status.response.left = (frame.data[2] << 8) | frame.data[3];
+			orte->jaws_status.flags.left = frame.data[4];
+			ORTEPublicationSend(orte->publication_jaws_status);
+			break;
+                case CAN_JAW_RIGHT_STATUS:
+                        orte->jaws_status.act_pos.right = (frame.data[0] << 8) | frame.data[1];
+                        orte->jaws_status.response.right = (frame.data[2] << 8) | frame.data[3];
+                        orte->jaws_status.flags.right = frame.data[4];
+                        ORTEPublicationSend(orte->publication_jaws_status);
+                break;
+                case CAN_LIFT_STATUS:
+                        orte->lift_status.act_pos = (frame.data[0] << 8) | frame.data[1];
+                        orte->lift_status.response = (frame.data[2] << 8) | frame.data[3];
+                        orte->lift_status.flags = frame.data[4];
+                        ORTEPublicationSend(orte->publication_lift_status);
+                break;
 		case CAN_ROBOT_CMD:
-			orte->robot_cmd.start = frame.data[0];
-			orte->robot_cmd.stop = frame.data[1];
+			orte->robot_cmd.start_condition = frame.data[0];
 			ORTEPublicationSend(orte->publication_robot_cmd);
 			break;
-
-		/* global sampling period -- time trigger */
-		case CAN_CORR_TRIG:
-			orte->corr_trig.seq = frame.data[0];
-			ORTEPublicationSend(orte->publication_corr_trigger);
+		case CAN_ROBOT_SWITCHES:
+			orte->robot_switches.team_color = (frame.data[0] & CAN_SWITCH_COLOR) ? 1 : 0;
+			orte->robot_switches.strategy = (frame.data[0] & CAN_SWITCH_STRATEGY ? 1 : 0);
+			ORTEPublicationSend(orte->publication_robot_switches);
 			break;
+		case CAN_ROBOT_BUMPERS:
+			orte->robot_bumpers.bumper_left = (frame.data[0] & CAN_BUMPER_LEFT) ? 0 : 1;
+			orte->robot_bumpers.bumper_left_across = (frame.data[0] & CAN_BUMPER_LEFT_ACROSS) ? 0 : 1;
+			orte->robot_bumpers.bumper_right = (frame.data[0] & CAN_BUMPER_RIGHT) ? 0 : 1;
+			orte->robot_bumpers.bumper_right_across = (frame.data[0] & CAN_BUMPER_RIGHT_ACROSS) ? 0 : 1;
+			orte->robot_bumpers.bumper_rear_left = (frame.data[0] & CAN_BUMPER_REAR_LEFT) ? 0 : 1;
+			orte->robot_bumpers.bumper_rear_right = (frame.data[0] & CAN_BUMPER_REAR_RIGHT) ? 0 : 1;
+			ORTEPublicationSend(orte->publication_robot_bumpers);
+		break;
 
-		/* distances measured by ULoPoS */
-		case CAN_CORR_DIST:
-			orte->corr_trig.t1 = (frame.data[1]<<8)|(frame.data[0]);
-			orte->corr_trig.t2 = (frame.data[3]<<8)|(frame.data[2]);
-			orte->corr_trig.t3 = (frame.data[5]<<8)|(frame.data[4]);
-			ORTEPublicationSend(orte->publication_corr_distances);
+		/* positioning by odometry */
+		case CAN_ODO_DATA:
+			orte->odo_data.right =
+					((frame.data[0]<<24)|
+					 (frame.data[1]<<16)|
+					 (frame.data[2]<<8));
+			orte->odo_data.left =
+					((frame.data[3]<<24)|
+					 (frame.data[4]<<16)|
+					 (frame.data[5]<<8));
+			ORTEPublicationSend(orte->publication_odo_data);
 			break;
 
 		/* positioning by odometry */
 		case CAN_MOTION_ODOMETRY_SIMPLE:
-			orte->motion_irc.right = 
-					((frame.data[0]<<24)|(frame.data[1]<<16)|
-					(frame.data[2]<<8)|(frame.data[3]));
-			orte->motion_irc.left = 
-					((frame.data[4]<<24)|(frame.data[5]<<16)|
-					 (frame.data[6]<<8)|(frame.data[7]));
+			orte->motion_irc.left =
+					((frame.data[0]<<24)|
+					 (frame.data[1]<<16)|
+					 (frame.data[2]<<8));
+			orte->motion_irc.right =
+					((frame.data[3]<<24)|
+					 (frame.data[4]<<16)|
+					 (frame.data[5]<<8));
+			orte->motion_irc.seq = frame.data[6];
 			ORTEPublicationSend(orte->publication_motion_irc);
 			break;
 
 		/* motion status */
 		case CAN_MOTION_STATUS:
-			orte->motion_status.err_left = 
+			orte->motion_status.err_left =
 					(frame.data[0]<<8)|(frame.data[1]);
-			orte->motion_status.err_right = 
+			orte->motion_status.err_right =
 					(frame.data[2]<<8)|(frame.data[3]);
-			if(++status_cnt == 5) {
+			if(++status_cnt == 2) { // h8eurobot sends duplicite can messages
 				ORTEPublicationSend(orte->publication_motion_status);
 				status_cnt = 0;
 			}
 			break;
+                /* voltage measurements from power board */
 		case CAN_PWR_ADC1:
 			double volt33, voltBAT;
 			voltBAT = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 				 (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 			volt33 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 				 (frame.data[6] << 8) | (frame.data[7]))/10000.0;
-		//	printf("PWR_ADC1: 3,3V = %2.2f, BAT = %2.2f\n",volt33,voltBAT);
 			orte->pwr_voltage.voltage33 = volt33;
 			orte->pwr_voltage.voltageBAT = voltBAT;
 			break;
@@ -279,121 +257,27 @@ int cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
 				 (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 			volt80 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 				 (frame.data[6] << 8) | (frame.data[7]))/10000.0;
-		//	printf("PWR_ADC2: 8,0V = %2.2f, 5,0V = %2.2f\n",volt80,volt50);
 			orte->pwr_voltage.voltage80 = volt80;
 			orte->pwr_voltage.voltage50 = volt50;
 
 			ORTEPublicationSend(orte->publication_pwr_voltage);
 			
 			break;
-		case CAN_ADC_1:
-			orte->sharps.front_right = s_gp2y0a21_ir2mmLong((frame.data[0] << 8)| \
-							  (frame.data[1]))/1000.0;
-			orte->sharps.right = s_gp2y0a21_ir2mmLong((frame.data[2] << 8)| \
-							  (frame.data[3]))/1000.0;
-			orte->sharps.left = s_gp2y0a21_ir2mmLong((frame.data[4] << 8)| \
-							  (frame.data[5]))/1000.0;
-			orte->sharps.front_left = s_gp2y0a21_ir2mmLong((frame.data[6] << 8)| \
-							  (frame.data[7]))/1000.0;
-			ORTEPublicationSend(orte->publication_sharps);
-			break;
-
-		case CAN_ADC_2:
-			orte->sharps.back_left = s_gp2d120_ir2mmShort((frame.data[4] << 8)| \
-							  (frame.data[5]))/1000.0;
-			orte->sharps.back_right = s_gp2d120_ir2mmShort((frame.data[6] << 8)| \
-							  (frame.data[7]))/1000.0;
-			ORTEPublicationSend(orte->publication_sharps);
-			break;
-		/* laser data */
-		case CAN_LAS1:
-//			printf("CAN: ");
-//			for (i=0; i<frame.can_dlc; i++) {
-//				printf("0x%02x ", frame.data[i]);
-//			}
-//			printf("can_dlc=%d\n", frame.can_dlc);
-			orte->laser_meas.cnt = frame.data[1];
-			las_bcnt = orte->laser_meas.cnt;
-			last_id = frame.data[0];
-			las_di = 4;
-			/* rotation period */
-			orte->laser_meas.period = (frame.data[2]<<8)|(frame.data[3]);
-//			printf("CAN ID=0x%02x: cnt=%d period=%d measures: ", 
-//					frame.can_id, orte->laser_meas.cnt, orte->laser_meas.period);
-
-			for (las_mi=0; las_mi<(frame.can_dlc-4)/2; las_mi++) {
-				switch (las_mi) {
-					case 0: las_meas = &orte->laser_meas.measures0; break;
-					case 1: las_meas = &orte->laser_meas.measures1; break;
-					default: break;
-				}
-				*las_meas = 
-					(frame.data[las_di++]<<8)|(frame.data[las_di++]);
-//				printf("0x%02x 0x%02x %u ", 
-//					frame.data[las_di-2], frame.data[las_di-1], *las_meas);
-			}
-//			printf("\n");
-			break;
-		case CAN_LAS2:
-		case CAN_LAS3:
-		case CAN_LAS4:
-//			printf("CAN: ");
-//			for (i=0; i<frame.can_dlc; i++) {
-//				printf("0x%02x ", frame.data[i]);
-//			}
-//			printf("can_dlc=%d\n", frame.can_dlc);
-//			printf("CAN ID=0x%02x: cnt=%d period=%d measures: ", 
-//				frame.can_id, orte->laser_meas.cnt, orte->laser_meas.period);
-			if (frame.data[0] != (last_id+(frame.can_id-CAN_LAS1)))
-				break;
-			las_di = 2;
-			while (las_di < 8 && las_bcnt > 0) {
-				switch (las_mi) {
-					case 2: las_meas = &orte->laser_meas.measures2; break;
-					case 3: las_meas = &orte->laser_meas.measures3; break;
-					case 4: las_meas = &orte->laser_meas.measures4; break;
-					case 5: las_meas = &orte->laser_meas.measures5; break;
-					case 6: las_meas = &orte->laser_meas.measures6; break;
-					case 7: las_meas = &orte->laser_meas.measures7; break;
-					case 8: las_meas = &orte->laser_meas.measures8; break;
-					case 9: las_meas = &orte->laser_meas.measures9; break;
-					default: break;
-				}
-				*las_meas = 
-					(frame.data[las_di++]<<8)|(frame.data[las_di++]);
-//				printf("0x%02x 0x%02x %u ", 
-//					frame.data[las_di-2], frame.data[las_di-1], *las_meas);
-				las_mi++;
-				las_bcnt--;
-			}
-			printf("\n");
-			if (las_bcnt == 0) {
-//				printf("ORTEPublicationSend()\n");
-				ORTEPublicationSend(orte->publication_laser_meas);
-			}
-			break;
-
-		case CAN_LAS_DATA:
-			orte->laser_data.period = (frame.data[0]<<8)|(frame.data[1]);
-			orte->laser_data.measure = (frame.data[2]<<8)|(frame.data[3]);
-			if (prev_meas != orte->laser_data.measure) {
-				ORTEPublicationSend(orte->publication_laser_data);
-				prev_meas = orte->laser_data.measure;
-			}
-			break;
 
-		case CAN_CMU:
-			orte->cmu.color = frame.data[0];
-			ORTEPublicationSend(orte->publication_cmu);
-			break;
+                case CAN_PWR_ALERT:
+                        orte->pwr_alert.alert_33 = (frame.data[0] & CAN_PWR_ALERT_33 ? 1 : 0);
+                        orte->pwr_alert.alert_50 = (frame.data[0] & CAN_PWR_ALERT_50 ? 1 : 0);
+                        orte->pwr_alert.alert_80 = (frame.data[0] & CAN_PWR_ALERT_80 ? 1 : 0);
+                        orte->pwr_alert.bat_full = (frame.data[0] & CAN_PWR_BATT_FULL ? 1 : 0);
+                        orte->pwr_alert.bat_mean = (frame.data[0] & CAN_PWR_BATT_MEAN ? 1 : 0);
+                        orte->pwr_alert.bat_low = (frame.data[0] & CAN_PWR_BATT_LOW ? 1 : 0);
+                        orte->pwr_alert.bat_critical = (frame.data[0] & CAN_PWR_BATT_CRITICAL ? 1 : 0);
 
-		case CAN_BUMPER:
-			orte->bumper.left = frame.data[0] & LEFT_BUMPER;
-			orte->bumper.right = frame.data[0] & RIGHT_BUMPER;
-			ORTEPublicationSend(orte->publication_bumper);
-			break;
+                        ORTEPublicationSend(orte->publication_pwr_alert);
+                        break;
 
 		default:
+			//FIXME: add logging here (Filip)
 //			printf("received CAN msg with unknown id: %x\n",frame.can_id);
 			break;
 	}
@@ -428,20 +312,6 @@ void rcv_can_msg_cb (const ORTERecvInfo *info, void *vinstance,
 			break;
 	}
 }
-void rcv_pwr_alert_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_pwr_alert(orte_data);
-			break;
-		case DEADLINE:
-			//printf("ORTE deadline occurred - PWR_CTRL receive\n");
-			break;
-	}
-}
 
 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance, 
 				void *recvCallBackParam)
@@ -451,13 +321,13 @@ void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 	switch (info->status) {
 		case NEW_DATA:
 			/* reversing motion direction, as it is different, than last year */
-			orte_data->motion_speed.left *= -1;
-			orte_data->motion_speed.right *=-1;
+			orte_data->motion_speed.left *= 1;
+			orte_data->motion_speed.right *= 1;
 			set_motor_speed(orte_data);
 			/*printf("motor cmd received\n");*/
 			break;
 		case DEADLINE:
-			printf("motor cmd deadline occurred, stopping motors\n");
+			//printf("motor cmd deadline occurred, stopping motors\n");
 			orte_data->motion_speed.left = 0;
 			orte_data->motion_speed.right = 0;
 			set_motor_speed(orte_data);
@@ -465,62 +335,41 @@ void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 	}
 }
 
-void rcv_servos_cb (const ORTERecvInfo *info, void *vinstance, 
-			void *recvCallBackParam) 
+void rcv_jaws_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:
-			set_serva(orte_data);	
-			break;
-		case DEADLINE:
-//			printf("ORTE deadline occurred - servo receive\n");
-			break;
-	}
-}
-
-void rcv_laser_cmd_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_laser_cmd(orte_data);	
+			set_jaws_cmd(orte_data);
 			break;
 		case DEADLINE:
-//			printf("ORTE deadline occurred - servo receive\n");
 			break;
 	}
 }
 
-void rcv_drives_cb (const ORTERecvInfo *info, void *vinstance, 
-			void *recvCallBackParam) 
+void rcv_lift_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:
-			set_drives(orte_data);	
-			break;
-		case DEADLINE:
-//			printf("ORTE deadline occurred - drives receive\n");
-			break;
-	}
+	        case NEW_DATA:
+	                set_lift_cmd(orte_data);
+	                break;
+	        case DEADLINE:
+	                break;
 }
+}
+
+struct robottype_orte_data orte;
 
 int main(int argc, char *argv[])
 {
-	fd_set read_fd_set;
-	struct timeval timeout;
 	int ret;
 	int size;
-	int rv = 0;
 
-	struct robottype_orte_data orte;
 	struct can_frame frame;
 
 	if (cand_init() < 0) {
@@ -530,48 +379,43 @@ int main(int argc, char *argv[])
 		printf("cand: init OK\n");
 	}
 
-	orte.strength = 1;
-
 	/* orte initialization */
-	robottype_roboorte_init(&orte);
+	if(robottype_roboorte_init(&orte)) {
+		printf("Roboorte initialization failed! Exiting...\n");
+		exit(-1);
+	}
+	else
+		printf("Roboorte OK\n");
 
 	/* creating publishers */
 	robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
 	robottype_publisher_motion_status_create(&orte, NULL, NULL);
+	robottype_publisher_odo_data_create(&orte, NULL, NULL);
 	robottype_publisher_motion_irc_create(&orte, NULL, NULL);
 	robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
-	robottype_publisher_laser_meas_create(&orte, NULL, NULL);
-	robottype_publisher_laser_data_create(&orte, NULL, NULL);
-	robottype_publisher_sharps_create(&orte, NULL, NULL);
-	robottype_publisher_cmu_create(&orte, NULL, NULL);
-	robottype_publisher_bumper_create(&orte, NULL, NULL);
+	robottype_publisher_robot_switches_create(&orte, NULL, NULL);
+	robottype_publisher_robot_bumpers_create(&orte, NULL, NULL);
+	robottype_publisher_jaws_status_create(&orte, NULL, NULL);
+        robottype_publisher_lift_status_create(&orte, NULL, NULL);
+	printf("Publishers OK\n");
 
 	/* 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_laser_cmd_create(&orte, 
-				rcv_laser_cmd_cb, &orte);
-
-	robottype_subscriber_drives_create(&orte, 
-				rcv_drives_cb, &orte);
+	robottype_subscriber_pwr_ctrl_create(&orte, rcv_pwr_ctrl_cb, &orte);
+	robottype_subscriber_motion_speed_create(&orte,	rcv_motion_speed_cb, &orte);
+	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);
+
+
+	printf("subscribers OK\n");
+
 	/* main loop */
 	for(;;) {
-		FD_ZERO(&read_fd_set);
-		FD_SET(sock, &read_fd_set);
-		timeout.tv_sec = 0;
-		timeout.tv_usec = 10000;
+		struct pollfd pfd[1] = {{sock, POLLIN, 0}};
 
-		ret = select(FD_SETSIZE, &read_fd_set, NULL, NULL, &timeout);
+		ret = poll(pfd, 1, -1);
 		if (ret < 0) {
-			perror("cand: select() error");
+			perror("cand: poll() error");
 			goto err;
 		}
 
@@ -579,19 +423,16 @@ int main(int argc, char *argv[])
                 if (ret == 0)
                         continue;
 
-                if (!FD_ISSET(sock, &read_fd_set))
-                        continue;
-
 		/* read data from SocketCAN */
 		size = read(sock, &frame, sizeof(struct can_frame));
 
 		/* parse data */
-		cand_parse_frame(&orte, frame);
+		if (size > 0)
+			cand_parse_frame(&orte, frame);
 	}
+	return EXIT_SUCCESS;
 
 err:
 	close(sock);
 	return 1;
-out:
-	return EXIT_SUCCESS;
 }