X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/blobdiff_plain/beb053d2fba4d6f1ec1e0613fb762d99da316411..16d700e75c6c24612de49c1370eb6c293de695df:/src/cand/cand.cc diff --git a/src/cand/cand.cc b/src/cand/cand.cc index 0412d91c..c36836da 100644 --- a/src/cand/cand.cc +++ b/src/cand/cand.cc @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -28,7 +29,6 @@ #include #include #include -#include #include #include #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) @@ -125,118 +104,40 @@ 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) +int set_jaws_cmd(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); + unsigned char data[3]; - return 0; -} + 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); -/** - * FIXME: this function is temporary, just for time LPCs are not ready - * Sends #CAN_PICKER message. - * - * - data[0] = orte_data->picker.rightchela; - * - data[1] = orte_data->picker.leftchela; - * - data[2] = orte_data->picker.leftbelt; - * - data[3] = orte_data->picker.rightbelt; - * - data[4] = orte_data->picker.timeout; - */ -int set_picker(struct robottype_orte_data *orte_data) -{ - unsigned char data[5]; - - data[0] = orte_data->picker.rightchela; - data[1] = orte_data->picker.leftchela; - data[2] = orte_data->picker.leftbelt; - data[3] = orte_data->picker.rightbelt; - data[4] = orte_data->picker.timeout; - can_send(CAN_PICKER, sizeof(data), data); - - return 0; -} - -/** - * Sends #CAN_CHELAE message. - * - * - data[1] = orte_data->chelae.left; - * - data[0] = orte_data->chelae.right; - */ -int set_chelae(struct robottype_orte_data *orte_data) -{ - unsigned char data[2]; - - data[0] = orte_data->chelae.left; - data[1] = orte_data->chelae.right; - can_send(CAN_CHELAE, sizeof(data), 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; } -/** - * Sends #CAN_BELTS message. - * - * - data[1] = orte_data->belts.left; - * - data[0] = orte_data->belts.right; - */ -int set_belts(struct robottype_orte_data *orte_data) +int set_lift_cmd(struct robottype_orte_data *orte_data) { - unsigned char data[2]; - - data[0] = orte_data->belts.left; - data[1] = orte_data->belts.right; - can_send(CAN_BELTS, 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]; + unsigned char data[4]; - data[0] = orte_data->pusher.pos >> 8; - data[1] = orte_data->pusher.pos & 0xFF; - can_send(CAN_PUSHER, sizeof(data), 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; @@ -258,64 +159,88 @@ int can_send(canid_t id, unsigned char length, unsigned char *data) /** * 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_trig); + 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_distances.t1 = (frame.data[1]<<8)|(frame.data[0]); - orte->corr_distances.t2 = (frame.data[3]<<8)|(frame.data[2]); - orte->corr_distances.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) | \ @@ -338,19 +263,21 @@ int cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame) ORTEPublicationSend(orte->publication_pwr_voltage); break; - 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 = s_gp2d120_ir2mmShort((frame.data[0]<<8)|(frame.data[1]))/1000.0; - ORTEPublicationSend(orte->publication_puck_distance); - break; - /* laser data */ - 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); + + 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; } @@ -385,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) @@ -408,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); @@ -422,105 +335,41 @@ void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance, } } -void rcv_lift_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_lift(orte_data); + set_jaws_cmd(orte_data); break; case DEADLINE: -// printf("ORTE deadline occurred - lift receive\n"); break; } } -void rcv_picker_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_picker(orte_data); - break; - case DEADLINE: -// printf("ORTE deadline occurred - chelae receive\n"); - break; - } + case NEW_DATA: + set_lift_cmd(orte_data); + break; + case DEADLINE: + break; } - -void rcv_chelae_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_chelae(orte_data); - break; - case DEADLINE: -// printf("ORTE deadline occurred - chelae receive\n"); - break; - } } -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_belts(orte_data); - break; - case DEADLINE: -// 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; - } -} +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,8 +379,6 @@ int main(int argc, char *argv[]) printf("cand: init OK\n"); } - orte.strength = 1; - /* orte initialization */ if(robottype_roboorte_init(&orte)) { printf("Roboorte initialization failed! Exiting...\n"); @@ -543,38 +390,32 @@ int main(int argc, char *argv[]) /* 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_cmu_create(&orte, NULL, NULL); - 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_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_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_lift_create(&orte, rcv_lift_cb, &orte); - robottype_subscriber_chelae_create(&orte, rcv_chelae_cb, &orte); - robottype_subscriber_belts_create(&orte, rcv_belts_cb, &orte); - robottype_subscriber_picker_create(&orte, rcv_picker_cb, &orte); // FIXME: temporary (LPCs not ready) - robottype_subscriber_holder_create(&orte, rcv_holder_cb, &orte); - robottype_subscriber_pusher_create(&orte, rcv_pusher_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; } @@ -582,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; }