-/*
- * cand_eb2007.cc 08/04/07
- *
- * CAN daemon for the Eurobot 2007 competition.
- *
- * Copyright: (c) 2007 DCE Eurobot Dragon Team
+/**
+ * @file cand.cc
+ * @date 08/04/08
+ *
+ * @brief CAN-ORTE bridge
+ *
+ * Copyright: (c) 2008 CTU Dragons
* CTU FEE - Department of Control Engineering
* License: GNU GPL v.2
*/
#include <config.h>
#endif
+#include <cstring>
#include <iostream>
#include <cstdlib>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <fcntl.h>
+#include <poll.h>
-#include "af_can.h"
-#include "lib.h"
+#include <af_can.h>
+#include <canutils.h>
#include <can_ids.h>
+#include <can_msg_masks.h>
+#include <can_msg_def.h>
#include <sharp.h>
#include <orte.h>
-#include <roboorte_eb2007.h>
-
+#include <roboorte_robottype.h>
+#include <can_msg_masks.h>
#include "cand.h"
int cand_init()
}
can_addr.can_family = AF_CAN;
- strcpy(can_ifreq.ifr_name, "can0");
+ std::strcpy(can_ifreq.ifr_name, "can0");
if (ioctl(sock, SIOCGIFINDEX, &can_ifreq) < 0) {
perror("cand: SIOCGIFINDEX");
return 0;
}
-int set_motor_speed(struct orte_data *orte_data)
+int set_pwr_ctrl(struct robottype_orte_data *orte_data)
+{
+ unsigned char data=0;
+ if(orte_data->pwr_ctrl.voltage33)
+ data |= CAN_PWR_CTRL_33_ON;
+ else
+ data |= CAN_PWR_CTRL_33_OFF;
+
+ if(orte_data->pwr_ctrl.voltage50)
+ data |= CAN_PWR_CTRL_50_ON;
+ else
+ data |= CAN_PWR_CTRL_50_OFF;
+
+ if(orte_data->pwr_ctrl.voltage80)
+ data |= CAN_PWR_CTRL_80_ON;
+ else
+ data |= CAN_PWR_CTRL_80_OFF;
+
+ can_send(CAN_PWR, 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);
+}
+
+int set_motor_speed(struct robottype_orte_data *orte_data)
{
unsigned char data[4];
- data[0] = orte_data->motion_speed.left >> 8;
- data[1] = orte_data->motion_speed.left & 0xff;
- data[2] = orte_data->motion_speed.right >> 8;
- data[3] = orte_data->motion_speed.right & 0xff;
+ data[0] = orte_data->motion_speed.right >> 8;
+ data[1] = orte_data->motion_speed.right & 0xff;
+ data[2] = orte_data->motion_speed.left >> 8;
+ 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;
}
-int set_serva(struct orte_data *orte_data)
+int set_jaws_cmd(struct robottype_orte_data *orte_data)
{
- unsigned char data[6];
+ unsigned char data[3];
- data[5] = orte_data->servos.backDoor;
- data[1] = orte_data->servos.frontDoor;
- data[2] = orte_data->servos.transporterFront;
- data[3] = orte_data->servos.transporterInner;
- data[4] = orte_data->servos.innerDoor;
- data[0] = orte_data->servos.release;
- can_send(CAN_SERVO, 6, data);
+ 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);
- /*for(int i=0; i<6; i++)
- printf("data %d = 0x%x\n", i, data[i]);*/
+ 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_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)
{
struct can_frame frame;
- int size;
+ size_t size;
frame.can_id = id;
frame.can_dlc = length;
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 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;
- int i;
+ static int status_cnt = 0;
switch(frame.can_id) {
+ /* 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_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.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_position.left =
- ((frame.data[0]<<24)|(frame.data[1]<<16)|
- (frame.data[2]<<8)|(frame.data[3]));
- orte->motion_position.right =
- ((frame.data[4]<<24)|(frame.data[5]<<16)|
- (frame.data[6]<<8)|(frame.data[7]));
- ORTEPublicationSend(orte->publicationMotionPos);
+ 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) {
- ORTEPublicationSend(orte->publicationMotionStatus);
+ if(++status_cnt == 2) { // h8eurobot sends duplicite can messages
+ ORTEPublicationSend(orte->publication_motion_status);
status_cnt = 0;
}
- /*
- if(ms.err_left || ms.err_right)
- printf("MOTOR STATUS: left 0x%x, right 0x%x\n",
- ms.err_left, ms.err_right);
- */
break;
-
- /* long sharps */
- case CAN_ADC_1:
- /* TODO: zkontrolovat, zda hodnoty jsou spravne v mm !! */
- orte->sharps_oponent.longSharpDist1 =
- s_ir2mmLong((frame.data[0]<<8)|(frame.data[1]))/1000.0;
- orte->sharps_oponent.longSharpDist2 =
- s_ir2mmLong((frame.data[2]<<8)|(frame.data[3]))/1000.0;
- orte->sharps_oponent.longSharpDist3 =
- s_ir2mmLong((frame.data[4]<<8)|(frame.data[5]))/1000.0;
- orte->front_door.state = (frame.data[6]<<8)|frame.data[7];
-
- if(++adc1_cnt == 5) {
- ORTEPublicationSend(orte->publicationSharp1);
- adc1_cnt = 0;
- }
+ /* 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;
+ orte->pwr_voltage.voltage33 = volt33;
+ orte->pwr_voltage.voltageBAT = voltBAT;
break;
- /* short sharps */
- case CAN_ADC_2:
- orte->sharps_waste.short1 = (frame.data[0]<<8)|(frame.data[1]);
- orte->sharps_waste.short2 = (frame.data[2]<<8)|(frame.data[3]);
- orte->sharps_waste.short3 = (frame.data[4]<<8)|(frame.data[5]);
- orte->sharps_waste.short4 = (frame.data[6]<<8)|(frame.data[7]);
-
- if(++adc2_cnt == 5) {
- ORTEPublicationSend(orte->publicationSharp2);
- adc2_cnt = 0;
- }
- break;
+ case CAN_PWR_ADC2:
+ double volt80, volt50;
+ volt50 = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
+ (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;
+ orte->pwr_voltage.voltage80 = volt80;
+ orte->pwr_voltage.voltage50 = volt50;
- /* sharps */
- case CAN_ADC_3:
- orte->adcs.sharpLong1 = (frame.data[0]);
- orte->adcs.sharpLong2 = (frame.data[1]);
- orte->adcs.sharpLong3 = (frame.data[2]);
- orte->adcs.sharpShort1 = (frame.data[3]);
- orte->adcs.sharpShort2 = (frame.data[4]);
- orte->adcs.sharpShort3 = (frame.data[5]);
- orte->adcs.sharpShort4 = (frame.data[6]);
- orte->adcs.frontDoor = (frame.data[7]);
+ ORTEPublicationSend(orte->publication_pwr_voltage);
- if(++adc3_cnt == 5) {
- ORTEPublicationSend(orte->publicationAdcs);
- adc3_cnt = 0;
- }
break;
- /* IR inner sensors */
- case CAN_IR:
- orte->inner_ir.front = frame.data[1];
- orte->inner_ir.back = frame.data[0];
- ORTEPublicationSend(orte->publicationIR);
+ 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);
- orte->dig_in.state = frame.data[2];
- if(++ir_cnt == 5) {
- ORTEPublicationSend(orte->publicationDI);
- ir_cnt = 0;
- }
- break;
+ ORTEPublicationSend(orte->publication_pwr_alert);
+ 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.cnt = frame.data[1];
- las_bcnt = orte->laser.cnt;
- last_id = frame.data[0];
- las_di = 4;
- /* rotation period */
- orte->laser.period = (frame.data[2]<<8)|(frame.data[3]);
- printf("CAN ID=0x%02x: cnt=%d period=%d measures: ",
- frame.can_id, orte->laser.cnt, orte->laser.period);
-
- for (las_mi=0; las_mi<(frame.can_dlc-4)/2; las_mi++) {
- switch (las_mi) {
- case 0: las_meas = &orte->laser.measures0; break;
- case 1: las_meas = &orte->laser.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.cnt, orte->laser.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.measures2; break;
- case 3: las_meas = &orte->laser.measures3; break;
- case 4: las_meas = &orte->laser.measures4; break;
- case 5: las_meas = &orte->laser.measures5; break;
- case 6: las_meas = &orte->laser.measures6; break;
- case 7: las_meas = &orte->laser.measures7; break;
- case 8: las_meas = &orte->laser.measures8; break;
- case 9: las_meas = &orte->laser.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->publicationLaser);
- }
- break;
default:
+ //FIXME: add logging here (Filip)
// printf("received CAN msg with unknown id: %x\n",frame.can_id);
break;
}
}
-void rcv_servos_cb (const ORTERecvInfo *info, void *vinstance,
+void rcv_pwr_ctrl_cb (const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
- struct orte_data *orte_data = (struct orte_data *)recvCallBackParam;
+ struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
switch (info->status) {
case NEW_DATA:
- set_serva(orte_data);
+ set_pwr_ctrl(orte_data);
break;
case DEADLINE:
- printf("ORTE deadline occurred - servo receive\n");
+ //printf("ORTE deadline occurred - PWR_CTRL receive\n");
+ break;
+ }
+}
+
+void rcv_can_msg_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:
+ send_can_msg(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)
{
- struct orte_data *orte_data = (struct orte_data *)recvCallBackParam;
+ struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
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;
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);
}
}
+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_jaws_cmd(orte_data);
+ break;
+ case DEADLINE:
+ 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;
+}
+}
+
int main(int argc, char *argv[])
{
- fd_set read_fd_set;
- struct timeval timeout;
int ret;
int size;
- struct orte_data orte;
+ struct robottype_orte_data orte;
struct can_frame frame;
if (cand_init() < 0) {
printf("cand: init OK\n");
}
- /* orte initialization */
- roboorte_init(NULL, &orte);
- /* publishers registration */
- roboorte_register_publisher(&orte,
- ORTE_PUB_MOTSTAT | ORTE_PUB_POSITION |
- ORTE_PUB_FD | ORTE_PUB_IR |
- ORTE_PUB_SHARPLONG | ORTE_PUB_SHARPSHORT |
- ORTE_PUB_DI | ORTE_PUB_ADCS | ORTE_PUB_LASER);
- /* subscribers registration */
-
- orte.rcv_motion_speed_cb = rcv_motion_speed_cb;
- orte.rcv_servos_cb = rcv_servos_cb;
-
- roboorte_register_subscriber(&orte,
- ORTE_SUB_MOTOR | ORTE_SUB_SERVOS);
+ orte.strength = 1;
+ /* orte initialization */
+ 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_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);
+
+
+ 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;
}
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;
}
projects / eurobot / public.git / blobdiff