X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/blobdiff_plain/8242c507a9f14a9220e6c2a2129b7cf8b3402cf4..16d700e75c6c24612de49c1370eb6c293de695df:/src/cand/cand.cc diff --git a/src/cand/cand.cc b/src/cand/cand.cc index fb76a16b..c36836da 100644 --- a/src/cand/cand.cc +++ b/src/cand/cand.cc @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -85,16 +86,6 @@ 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) { return can_send(orte_data->can_msg.id, orte_data->can_msg.len, orte_data->can_msg.data); @@ -113,19 +104,35 @@ int set_motor_speed(struct robottype_orte_data *orte_data) return 0; } -/** - * Sends #CAN_HOKUYO_PITCH message. - * - * - data = orte_data->pusher.pos - */ -int set_hokuyo_pitch(struct robottype_orte_data *orte_data) +int set_jaws_cmd(struct robottype_orte_data *orte_data) { - unsigned char data = orte_data->hokuyo_pitch.angle; + unsigned char data[3]; + + 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->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); - can_send(CAN_HOKUYO_PITCH, sizeof(data), &data); return 0; } +int set_lift_cmd(struct robottype_orte_data *orte_data) +{ + unsigned char data[4]; + + 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; +} int can_send(canid_t id, unsigned char length, unsigned char *data) { @@ -154,25 +161,54 @@ int can_send(canid_t id, unsigned char length, unsigned char *data) */ void cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame) { - int status_cnt = 0; + 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; + 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; /* positioning by odometry */ case CAN_ODO_DATA: - orte->odo_data.left = + orte->odo_data.right = ((frame.data[0]<<24)| (frame.data[1]<<16)| (frame.data[2]<<8)); - orte->odo_data.right = + orte->odo_data.left = ((frame.data[3]<<24)| (frame.data[4]<<16)| (frame.data[5]<<8)); @@ -181,11 +217,11 @@ void cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame) /* positioning by odometry */ case CAN_MOTION_ODOMETRY_SIMPLE: - orte->motion_irc.right = + orte->motion_irc.left = ((frame.data[0]<<24)| (frame.data[1]<<16)| (frame.data[2]<<8)); - orte->motion_irc.left = + orte->motion_irc.right = ((frame.data[3]<<24)| (frame.data[4]<<16)| (frame.data[5]<<8)); @@ -195,15 +231,16 @@ void cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame) /* 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) | \ @@ -226,6 +263,19 @@ void cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame) ORTEPublicationSend(orte->publication_pwr_voltage); 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); @@ -262,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) @@ -285,8 +321,8 @@ 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; @@ -299,30 +335,41 @@ void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance, } } -void rcv_hokuyo_pitch_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_hokuyo_pitch(orte_data); + set_jaws_cmd(orte_data); break; case DEADLINE: -// printf("ORTE deadline occurred - hokuyo pitch receive\n"); break; } } +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_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; - struct robottype_orte_data orte; struct can_frame frame; if (cand_init() < 0) { @@ -332,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"); @@ -348,28 +393,29 @@ int main(int argc, char *argv[]) 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_actuator_status_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_hokuyo_pitch_create(&orte, rcv_hokuyo_pitch_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; } @@ -377,14 +423,12 @@ 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;