src/cand/cand.cc

   1 /**
   2  * @file   cand.cc
   3  * @date   08/04/08
   4  *
   5  * @brief  CAN-ORTE bridge
   6  *
   7  * Copyright: (c) 2008 CTU Dragons
   8  *            CTU FEE - Department of Control Engineering
   9  * License: GNU GPL v.2
  10  */
  11
  12 #ifdef HAVE_CONFIG_H
  13 #include <config.h>
  14 #endif
  15
  16 #include <cstring>
  17 #include <iostream>
  18 #include <cstdlib>
  19 #include <sys/socket.h>
  20 #include <sys/ioctl.h>
  21 #include <fcntl.h>
  22 #include <poll.h>
  23
  24 #include <af_can.h>
  25 #include <canutils.h>
  26
  27 #include <can_ids.h>
  28 #include <can_msg_masks.h>
  29 #include <can_msg_def.h>
  30 #include <sharp.h>
  31 #include <orte.h>
  32 #include <roboorte_robottype.h>
  33 #include <can_msg_masks.h>
  34 #include "cand.h"
  35
  36 int cand_init()
  37 {
  38         if ((sock = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
  39                 perror("cand: unable to create a socket");
  40                 return -1;
  41         }
  42
  43         can_addr.can_family = AF_CAN;
  44         std::strcpy(can_ifreq.ifr_name, "can0");
  45
  46         if (ioctl(sock, SIOCGIFINDEX, &can_ifreq) < 0) {
  47                 perror("cand: SIOCGIFINDEX");
  48                 return -2;
  49         }
  50
  51         can_addr.can_ifindex = can_ifreq.ifr_ifindex;
  52
  53         if (!can_addr.can_ifindex) {
  54                 perror("cand: invalid socket interface");
  55                 return -3;
  56         }
  57
  58         if (bind(sock, (struct sockaddr *)&can_addr, sizeof(can_addr)) < 0) {
  59                 perror("cand: unable to bind");
  60                 return -4;
  61         }
  62
  63         return 0;
  64 }
  65
  66 int set_pwr_ctrl(struct robottype_orte_data *orte_data)
  67 {
  68         unsigned char data=0;
  69         if(orte_data->pwr_ctrl.voltage33)
  70                 data |= CAN_PWR_CTRL_33_ON;
  71         else
  72                 data |= CAN_PWR_CTRL_33_OFF;
  73
  74         if(orte_data->pwr_ctrl.voltage50)
  75                 data |= CAN_PWR_CTRL_50_ON;
  76         else
  77                 data |= CAN_PWR_CTRL_50_OFF;
  78
  79         if(orte_data->pwr_ctrl.voltage80)
  80                 data |= CAN_PWR_CTRL_80_ON;
  81         else
  82                 data |= CAN_PWR_CTRL_80_OFF;
  83
  84         can_send(CAN_PWR, 1, &data);
  85
  86         return 0;
  87 }
  88
  89 int send_can_msg(struct robottype_orte_data *orte_data)
  90 {
  91         return can_send(orte_data->can_msg.id, orte_data->can_msg.len, orte_data->can_msg.data);
  92 }
  93
  94 int set_motor_speed(struct robottype_orte_data *orte_data)
  95 {
  96         unsigned char data[4];
  97
  98         data[0] = orte_data->motion_speed.right >> 8;
  99         data[1] = orte_data->motion_speed.right & 0xff;
 100         data[2] = orte_data->motion_speed.left >> 8;
 101         data[3] = orte_data->motion_speed.left & 0xff;
 102         can_send(CAN_MOTION_CMD, 4, data);
 103
 104         return 0;
 105 }
 106
 107 int set_jaws_cmd(struct robottype_orte_data *orte_data)
 108 {
 109         unsigned char data[3];
 110
 111         data[0] = orte_data->jaws_cmd.req_pos.left >> 8;
 112         data[1] = orte_data->jaws_cmd.req_pos.left & 0xff;
 113         data[2] = orte_data->jaws_cmd.speed.left;
 114         can_send(CAN_JAW_LEFT_CMD, 3, data);
 115
 116         data[0] = orte_data->jaws_cmd.req_pos.right >> 8;
 117         data[1] = orte_data->jaws_cmd.req_pos.right & 0xff;
 118         data[2] = orte_data->jaws_cmd.speed.right;
 119         can_send(CAN_JAW_RIGHT_CMD, 3, data);
 120
 121         return 0;
 122 }
 123
 124 int set_lift_cmd(struct robottype_orte_data *orte_data)
 125 {
 126         unsigned char data[4];
 127
 128         data[0] = orte_data->lift_cmd.req_pos >> 8;
 129         data[1] = orte_data->lift_cmd.req_pos & 0xff;
 130         data[2] = orte_data->lift_cmd.speed;
 131         data[3] = orte_data->lift_cmd.homing;
 132         can_send(CAN_LIFT_CMD, 4, data);
 133
 134         return 0;
 135 }
 136
 137 int can_send(canid_t id, unsigned char length, unsigned char *data)
 138 {
 139         struct can_frame frame;
 140         size_t size;
 141
 142         frame.can_id = id;
 143         frame.can_dlc = length;
 144
 145         for(int i=0; i<length; i++)
 146                 frame.data[i] = data[i];
 147
 148         if ((size = write(sock, &frame, sizeof(struct can_frame))) < 0) {
 149                 perror("cand: can send: write()");
 150                 return -1;
 151         } else if (size < sizeof(struct can_frame)) {
 152                 perror("cand: can send: incomplete CAN frame\n");
 153                 return 1;
 154         }
 155
 156         return 0;
 157 }
 158
 159 /**
 160  * Parse frame ID and react as required
 161  */
 162 void cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
 163 {
 164         static int status_cnt = 0;
 165
 166         switch(frame.can_id) {
 167                 /* robot commands (start, ..) */
 168                 case CAN_CL_SENSOR_STATUS:
 169                         orte->cl_sensor_status.pattern_match = frame.data[0];
 170                         ORTEPublicationSend(orte->publication_cl_sensor_status);
 171                         break;
 172                 case CAN_JAW_LEFT_STATUS:
 173                         orte->jaws_status.act_pos.left = (frame.data[0] << 8) | frame.data[1];
 174                         orte->jaws_status.response.left = (frame.data[2] << 8) | frame.data[3];
 175                         orte->jaws_status.flags.left = frame.data[4];
 176                         ORTEPublicationSend(orte->publication_jaws_status);
 177                         break;
 178                 case CAN_JAW_RIGHT_STATUS:
 179                         orte->jaws_status.act_pos.right = (frame.data[0] << 8) | frame.data[1];
 180                         orte->jaws_status.response.right = (frame.data[2] << 8) | frame.data[3];
 181                         orte->jaws_status.flags.right = frame.data[4];
 182                         ORTEPublicationSend(orte->publication_jaws_status);
 183                 break;
 184                 case CAN_LIFT_STATUS:
 185                         orte->lift_status.act_pos = (frame.data[0] << 8) | frame.data[1];
 186                         orte->lift_status.response = (frame.data[2] << 8) | frame.data[3];
 187                         orte->lift_status.flags = frame.data[4];
 188                         ORTEPublicationSend(orte->publication_lift_status);
 189                 break;
 190                 case CAN_ROBOT_CMD:
 191                         orte->robot_cmd.start_condition = frame.data[0];
 192                         ORTEPublicationSend(orte->publication_robot_cmd);
 193                         break;
 194                 case CAN_ROBOT_SWITCHES:
 195                         /*
 196                         char tmp_color = 0;
 197                         if (frame.data[0] & CAN_SWITCH_COLOR_0) tmp_color += 1;
 198                         if (frame.data[0] & CAN_SWITCH_COLOR_1) tmp_color += 2;
 199                         orte->robot_switches.team_color = tmp_color;
 200                         */
 201                         orte->robot_switches.team_color = frame.data[0] & (CAN_SWITCH_COLOR_0 + CAN_SWITCH_COLOR_1);
 202                         orte->robot_switches.strategy = (frame.data[0] & CAN_SWITCH_STRATEGY ? 1 : 0);
 203                         orte->robot_switches.home = (frame.data[0] & CAN_SWITCH_HOME ? 1 : 0);
 204                         ORTEPublicationSend(orte->publication_robot_switches);
 205                         break;
 206                 case CAN_ROBOT_BUMPERS:
 207                         orte->robot_bumpers.bumper_left = (frame.data[0] & CAN_BUMPER_LEFT) ? 0 : 1;
 208                         orte->robot_bumpers.bumper_left_across = (frame.data[0] & CAN_BUMPER_LEFT_ACROSS) ? 0 : 1;
 209                         orte->robot_bumpers.bumper_right = (frame.data[0] & CAN_BUMPER_RIGHT) ? 0 : 1;
 210                         orte->robot_bumpers.bumper_right_across = (frame.data[0] & CAN_BUMPER_RIGHT_ACROSS) ? 0 : 1;
 211                         orte->robot_bumpers.bumper_rear_left = (frame.data[0] & CAN_BUMPER_REAR_LEFT) ? 0 : 1;
 212                         orte->robot_bumpers.bumper_rear_right = (frame.data[0] & CAN_BUMPER_REAR_RIGHT) ? 0 : 1;
 213                         ORTEPublicationSend(orte->publication_robot_bumpers);
 214                 break;
 215
 216                 /* positioning by independent odometry */
 217                 case CAN_ODO_DATA:
 218                         orte->odo_data.right =
 219                                         ((frame.data[0]<<24)|
 220                                          (frame.data[1]<<16)|
 221                                          (frame.data[2]<<8));
 222                         orte->odo_data.left =
 223                                         ((frame.data[3]<<24)|
 224                                          (frame.data[4]<<16)|
 225                                          (frame.data[5]<<8));
 226                         ORTEPublicationSend(orte->publication_odo_data);
 227                         break;
 228
 229                 /* positioning by odometry from motors*/
 230                 case CAN_MOTION_ODOMETRY_SIMPLE:
 231                         orte->motion_irc.left =
 232                                         ((frame.data[0]<<24)|
 233                                          (frame.data[1]<<16)|
 234                                          (frame.data[2]<<8));
 235                         orte->motion_irc.right =
 236                                         ((frame.data[3]<<24)|
 237                                          (frame.data[4]<<16)|
 238                                          (frame.data[5]<<8));
 239                         orte->motion_irc.seq = frame.data[6];
 240                         ORTEPublicationSend(orte->publication_motion_irc);
 241                         break;
 242
 243                 /* motion status */
 244                 case CAN_MOTION_STATUS:
 245                         orte->motion_status.err_left =
 246                                         (frame.data[0]<<8)|(frame.data[1]);
 247                         orte->motion_status.err_right =
 248                                         (frame.data[2]<<8)|(frame.data[3]);
 249                         if(++status_cnt == 2) { // h8eurobot sends duplicite can messages
 250                                 ORTEPublicationSend(orte->publication_motion_status);
 251                                 status_cnt = 0;
 252                         }
 253                         break;
 254                 /* voltage measurements from power board */
 255                 case CAN_PWR_ADC1:
 256                         double volt33, voltBAT;
 257                         voltBAT = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 258                                  (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 259                         volt33 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 260                                  (frame.data[6] << 8) | (frame.data[7]))/10000.0;
 261                         orte->pwr_voltage.voltage33 = volt33;
 262                         orte->pwr_voltage.voltageBAT = voltBAT;
 263                         break;
 264
 265                 case CAN_PWR_ADC2:
 266                         double volt80, volt50;
 267                         volt50 = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 268                                  (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 269                         volt80 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 270                                  (frame.data[6] << 8) | (frame.data[7]))/10000.0;
 271                         orte->pwr_voltage.voltage80 = volt80;
 272                         orte->pwr_voltage.voltage50 = volt50;
 273
 274                         ORTEPublicationSend(orte->publication_pwr_voltage);
 275
 276                         break;
 277
 278                 case CAN_PWR_ALERT:
 279                         orte->pwr_alert.alert_33 = (frame.data[0] & CAN_PWR_ALERT_33 ? 1 : 0);
 280                         orte->pwr_alert.alert_50 = (frame.data[0] & CAN_PWR_ALERT_50 ? 1 : 0);
 281                         orte->pwr_alert.alert_80 = (frame.data[0] & CAN_PWR_ALERT_80 ? 1 : 0);
 282                         orte->pwr_alert.bat_full = (frame.data[0] & CAN_PWR_BATT_FULL ? 1 : 0);
 283                         orte->pwr_alert.bat_mean = (frame.data[0] & CAN_PWR_BATT_MEAN ? 1 : 0);
 284                         orte->pwr_alert.bat_low = (frame.data[0] & CAN_PWR_BATT_LOW ? 1 : 0);
 285                         orte->pwr_alert.bat_critical = (frame.data[0] & CAN_PWR_BATT_CRITICAL ? 1 : 0);
 286
 287                         ORTEPublicationSend(orte->publication_pwr_alert);
 288                         break;
 289
 290                 default:
 291                         //FIXME: add logging here (Filip)
 292 //                      printf("received CAN msg with unknown id: %x\n",frame.can_id);
 293                         break;
 294         }
 295 }
 296
 297 void rcv_pwr_ctrl_cb (const ORTERecvInfo *info, void *vinstance,
 298                         void *recvCallBackParam)
 299 {
 300         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 301
 302         switch (info->status) {
 303                 case NEW_DATA:
 304                         set_pwr_ctrl(orte_data);
 305                         break;
 306                 case DEADLINE:
 307                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 308                         break;
 309         }
 310 }
 311
 312 void rcv_can_msg_cb (const ORTERecvInfo *info, void *vinstance,
 313                         void *recvCallBackParam)
 314 {
 315         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 316
 317         switch (info->status) {
 318                 case NEW_DATA:
 319                         send_can_msg(orte_data);
 320                         break;
 321                 case DEADLINE:
 322                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 323                         break;
 324         }
 325 }
 326
 327 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 328                                 void *recvCallBackParam)
 329 {
 330         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 331
 332         switch (info->status) {
 333                 case NEW_DATA:
 334                         /* reversing motion direction, as it is different, than last year */
 335                         orte_data->motion_speed.left *= 1;
 336                         orte_data->motion_speed.right *= 1;
 337                         set_motor_speed(orte_data);
 338                         /*printf("motor cmd received\n");*/
 339                         break;
 340                 case DEADLINE:
 341                         //printf("motor cmd deadline occurred, stopping motors\n");
 342                         orte_data->motion_speed.left = 0;
 343                         orte_data->motion_speed.right = 0;
 344                         set_motor_speed(orte_data);
 345                         break;
 346         }
 347 }
 348
 349 void rcv_jaws_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 350                       void *recvCallBackParam)
 351 {
 352         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 353
 354         switch (info->status) {
 355                 case NEW_DATA:
 356                         set_jaws_cmd(orte_data);
 357                         break;
 358                 case DEADLINE:
 359                         break;
 360         }
 361 }
 362
 363 void rcv_lift_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 364                       void *recvCallBackParam)
 365 {
 366         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 367
 368         switch (info->status) {
 369                 case NEW_DATA:
 370                         set_lift_cmd(orte_data);
 371                         break;
 372                 case DEADLINE:
 373                         break;
 374         }
 375 }
 376
 377 void rcv_cl_sensor_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 378                       void *recvCallBackParam)
 379 {
 380         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 381
 382         switch (info->status) {
 383                 case NEW_DATA:
 384                         unsigned char data[1];
 385                         data[0] = orte_data->cl_sensor_cmd.bank_nbr;
 386                         can_send(CAN_CL_SENSOR_CMD, 1, data);
 387                         break;
 388                 case DEADLINE:
 389                         break;
 390         }
 391 }
 392
 393 int main(int argc, char *argv[])
 394 {
 395         int ret;
 396         int size;
 397
 398         struct robottype_orte_data orte;
 399         struct can_frame frame;
 400
 401         if (cand_init() < 0) {
 402                 printf("cand: init failed\n");
 403                 exit(1);
 404         } else {
 405                 printf("cand: init OK\n");
 406         }
 407
 408         orte.strength = 1;
 409
 410         /* orte initialization */
 411         if(robottype_roboorte_init(&orte)) {
 412                 printf("Roboorte initialization failed! Exiting...\n");
 413                 exit(-1);
 414         }
 415         else
 416                 printf("Roboorte OK\n");
 417
 418         /* creating publishers */
 419         robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
 420         robottype_publisher_motion_status_create(&orte, NULL, NULL);
 421         robottype_publisher_odo_data_create(&orte, NULL, NULL);
 422         robottype_publisher_motion_irc_create(&orte, NULL, NULL);
 423         robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
 424         robottype_publisher_robot_switches_create(&orte, NULL, NULL);
 425         robottype_publisher_robot_bumpers_create(&orte, NULL, NULL);
 426         robottype_publisher_jaws_status_create(&orte, NULL, NULL);
 427         robottype_publisher_lift_status_create(&orte, NULL, NULL);
 428         robottype_publisher_cl_sensor_status_create(&orte, NULL, NULL);
 429         printf("Publishers OK\n");
 430
 431         /* creating subscribers */
 432         robottype_subscriber_pwr_ctrl_create(&orte, rcv_pwr_ctrl_cb, &orte);
 433         robottype_subscriber_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
 434         robottype_subscriber_jaws_cmd_create(&orte, rcv_jaws_cmd_cb, &orte);
 435         robottype_subscriber_lift_cmd_create(&orte, rcv_lift_cmd_cb, &orte);
 436         robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
 437         robottype_subscriber_cl_sensor_cmd_create(&orte, rcv_cl_sensor_cmd_cb, &orte);
 438         printf("subscribers OK\n");
 439
 440         /* main loop */
 441         for(;;) {
 442                 struct pollfd pfd[1] = {{sock, POLLIN, 0}};
 443
 444                 ret = poll(pfd, 1, -1);
 445                 if (ret < 0) {
 446                         perror("cand: poll() error");
 447                         goto err;
 448                 }
 449
 450                 /* timeout expired, nothing to be done */
 451                 if (ret == 0)
 452                         continue;
 453
 454                 /* read data from SocketCAN */
 455                 size = read(sock, &frame, sizeof(struct can_frame));
 456
 457                 /* parse data */
 458                 if (size > 0)
 459                         cand_parse_frame(&orte, frame);
 460         }
 461         return EXIT_SUCCESS;
 462
 463 err:
 464         close(sock);
 465         return 1;
 466 }