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
  23 #include <af_can.h>
  24 #include <canutils.h>
  25
  26 #include <can_ids.h>
  27 #include <can_msg_masks.h>
  28 #include <can_msg_def.h>
  29 #include <sharp.h>
  30 #include <orte.h>
  31 #include <roboorte.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 set_pwr_alert(struct robottype_orte_data *orte_data)
  90 {
  91         unsigned char data=0;
  92         data = orte_data->pwr_alert.value;
  93
  94         can_send(CAN_PWR_ALERT, 1, &data);
  95
  96         return 0;
  97 }
  98
  99 int send_can_msg(struct robottype_orte_data *orte_data)
 100 {
 101         uint8_t data[8];
 102
 103         data[0] = orte_data->can_msg.data1;
 104         data[1] = orte_data->can_msg.data2;
 105         data[2] = orte_data->can_msg.data3;
 106         data[3] = orte_data->can_msg.data4;
 107         data[4] = orte_data->can_msg.data5;
 108         data[5] = orte_data->can_msg.data6;
 109         data[6] = orte_data->can_msg.data7;
 110         data[7] = orte_data->can_msg.data8;
 111
 112         can_send(orte_data->can_msg.id, orte_data->can_msg.len, data);
 113 }
 114
 115 int set_motor_speed(struct robottype_orte_data *orte_data)
 116 {
 117         unsigned char data[4];
 118
 119         data[0] = orte_data->motion_speed.right >> 8;
 120         data[1] = orte_data->motion_speed.right & 0xff;
 121         data[2] = orte_data->motion_speed.left >> 8;
 122         data[3] = orte_data->motion_speed.left & 0xff;
 123         can_send(CAN_MOTION_CMD, 4, data);
 124
 125         return 0;
 126 }
 127
 128 /**
 129  * Sends #CAN_LIFT message.
 130  *
 131  * - data[1] = orte_data->lift.pos & 0xFF; // lower byte
 132  * - data[0] = orte_data->lift.pos >> 8; // upper byte
 133  */
 134 int set_lift(struct robottype_orte_data *orte_data)
 135 {
 136         unsigned char data[2];
 137
 138         data[0] = orte_data->lift.pos >> 8;
 139         data[1] = orte_data->lift.pos & 0xFF;
 140         can_send(CAN_LIFT, sizeof(data), data);
 141
 142         return 0;
 143 }
 144
 145 /**
 146  * Sends #CAN_CHELAE message.
 147  *
 148  * - data[1] = orte_data->chelae.left;
 149  * - data[0] = orte_data->chelae.right;
 150  */
 151 int set_chelae(struct robottype_orte_data *orte_data)
 152 {
 153         unsigned char data[2];
 154
 155         data[0] = orte_data->chelae.left;
 156         data[1] = orte_data->chelae.right;
 157         can_send(CAN_CHELAE, sizeof(data), data);
 158
 159         return 0;
 160 }
 161
 162 /**
 163  * Sends #CAN_BELTS message.
 164  *
 165  * - data[1] = orte_data->belts.left;
 166  * - data[0] = orte_data->belts.right;
 167  */
 168 int set_belts(struct robottype_orte_data *orte_data)
 169 {
 170         unsigned char data[2];
 171
 172         data[0] = orte_data->belts.left;
 173         data[1] = orte_data->belts.right;
 174         can_send(CAN_BELTS, sizeof(data), data);
 175
 176         return 0;
 177 }
 178
 179 /**
 180  * Sends #CAN_HOLDER message.
 181  *
 182  * - data[0] = orte_data->holder.pos;
 183  */
 184 int set_holder(struct robottype_orte_data *orte_data)
 185 {
 186         unsigned char data = orte_data->holder.pos;
 187         can_send(CAN_HOLDER, sizeof(data), &data);
 188
 189         return 0;
 190 }
 191
 192 /**
 193  * Sends #CAN_PUSHER message.
 194  *
 195  * - data[1] = orte_data->pusher.pos & 0xFF; // lower byte
 196  * - data[0] = orte_data->pusher.pos >> 8; // upper byte
 197  */
 198 int set_pusher(struct robottype_orte_data *orte_data)
 199 {
 200         unsigned char data[2];
 201
 202         data[0] = orte_data->pusher.pos >> 8;
 203         data[1] = orte_data->pusher.pos & 0xFF;
 204         can_send(CAN_PUSHER, sizeof(data), data);
 205
 206         return 0;
 207 }
 208
 209 /**
 210  * Sends #CAN_HOKUYO_PITCH message.
 211  *
 212  * - data = orte_data->pusher.pos
 213  */
 214 int set_hokuyo_pitch(struct robottype_orte_data *orte_data)
 215 {
 216         unsigned char data = orte_data->hokuyo_pitch.angle;
 217
 218         can_send(CAN_HOKUYO_PITCH, sizeof(data), &data);
 219         return 0;
 220 }
 221
 222
 223 int can_send(canid_t id, unsigned char length, unsigned char *data)
 224 {
 225         struct can_frame frame;
 226         int size;
 227
 228         frame.can_id = id;
 229         frame.can_dlc = length;
 230
 231         for(int i=0; i<length; i++)
 232                 frame.data[i] = data[i];
 233
 234         if ((size = write(sock, &frame, sizeof(struct can_frame))) < 0) {
 235                 perror("cand: can send: write()");
 236                 return -1;
 237         } else if (size < sizeof(struct can_frame)) {
 238                 perror("cand: can send: incomplete CAN frame\n");
 239                 return 1;
 240         }
 241
 242         return 0;
 243 }
 244
 245 /**
 246  * Parse frame ID and react as required
 247  */
 248 int cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
 249 {
 250         int status_cnt = 0;
 251         int adc1_cnt = 0;
 252         int adc2_cnt = 0;
 253         int adc3_cnt = 0;
 254         int ir_cnt = 0;
 255         unsigned short *las_meas;
 256         int las_mi, las_di, las_bcnt;
 257         static unsigned short prev_meas;
 258         int i;
 259
 260         switch(frame.can_id) {
 261                 /* voltage measurements from power board */
 262
 263                 /* robot commands (start, ..) */
 264                 case CAN_ROBOT_CMD:
 265                         orte->robot_cmd.start = frame.data[0];
 266                         orte->robot_cmd.stop = frame.data[1];
 267                         ORTEPublicationSend(orte->publication_robot_cmd);
 268                         break;
 269
 270                 /* global sampling period -- time trigger */
 271                 case CAN_CORR_TRIG:
 272                         orte->corr_trig.seq = frame.data[0];
 273                         ORTEPublicationSend(orte->publication_corr_trig);
 274                         break;
 275
 276                 /* distances measured by ULoPoS */
 277                 case CAN_CORR_DIST:
 278                         orte->corr_distances.t1 = (frame.data[1]<<8)|(frame.data[0]);
 279                         orte->corr_distances.t2 = (frame.data[3]<<8)|(frame.data[2]);
 280                         orte->corr_distances.t3 = (frame.data[5]<<8)|(frame.data[4]);
 281                         ORTEPublicationSend(orte->publication_corr_distances);
 282                         break;
 283
 284                 /* positioning by odometry */
 285                 case CAN_MOTION_ODOMETRY_SIMPLE:
 286                         orte->motion_irc.right =
 287                                         ((frame.data[0]<<24)|(frame.data[1]<<16)|
 288                                         (frame.data[2]<<8)|(frame.data[3]));
 289                         orte->motion_irc.left =
 290                                         ((frame.data[4]<<24)|(frame.data[5]<<16)|
 291                                          (frame.data[6]<<8)|(frame.data[7]));
 292                         ORTEPublicationSend(orte->publication_motion_irc);
 293                         break;
 294
 295                 /* motion status */
 296                 case CAN_MOTION_STATUS:
 297                         orte->motion_status.err_left =
 298                                         (frame.data[0]<<8)|(frame.data[1]);
 299                         orte->motion_status.err_right =
 300                                         (frame.data[2]<<8)|(frame.data[3]);
 301                         if(++status_cnt == 5) {
 302                                 ORTEPublicationSend(orte->publication_motion_status);
 303                                 status_cnt = 0;
 304                         }
 305                         break;
 306                 case CAN_PWR_ADC1:
 307                         double volt33, voltBAT;
 308                         voltBAT = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 309                                  (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 310                         volt33 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 311                                  (frame.data[6] << 8) | (frame.data[7]))/10000.0;
 312                         orte->pwr_voltage.voltage33 = volt33;
 313                         orte->pwr_voltage.voltageBAT = voltBAT;
 314                         break;
 315
 316                 case CAN_PWR_ADC2:
 317                         double volt80, volt50;
 318                         volt50 = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 319                                  (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 320                         volt80 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 321                                  (frame.data[6] << 8) | (frame.data[7]))/10000.0;
 322                         orte->pwr_voltage.voltage80 = volt80;
 323                         orte->pwr_voltage.voltage50 = volt50;
 324
 325                         ORTEPublicationSend(orte->publication_pwr_voltage);
 326
 327                         break;
 328                 case CAN_ADC_1: /* data from the sharp sensor measuring distance of the puck (column element) */
 329                         //orte->puck_distance.distance = ((frame.data[0]<<8)|(frame.data[1]));
 330                         orte->puck_distance.distance = puckSharp_ir2mm((frame.data[0]<<8)|(frame.data[1]))/1000.0;
 331                         ORTEPublicationSend(orte->publication_puck_distance);
 332                         break;
 333
 334                 /* laser data */
 335                 case CAN_CMU:
 336                         orte->cmu.color = frame.data[0];
 337                         ORTEPublicationSend(orte->publication_cmu);
 338                         break;
 339
 340                 default:
 341 //                      printf("received CAN msg with unknown id: %x\n",frame.can_id);
 342                         break;
 343         }
 344 }
 345
 346 void rcv_pwr_ctrl_cb (const ORTERecvInfo *info, void *vinstance,
 347                         void *recvCallBackParam)
 348 {
 349         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 350
 351         switch (info->status) {
 352                 case NEW_DATA:
 353                         set_pwr_ctrl(orte_data);
 354                         break;
 355                 case DEADLINE:
 356                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 357                         break;
 358         }
 359 }
 360
 361 void rcv_can_msg_cb (const ORTERecvInfo *info, void *vinstance,
 362                         void *recvCallBackParam)
 363 {
 364         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 365
 366         switch (info->status) {
 367                 case NEW_DATA:
 368                         send_can_msg(orte_data);
 369                         break;
 370                 case DEADLINE:
 371                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 372                         break;
 373         }
 374 }
 375 void rcv_pwr_alert_cb (const ORTERecvInfo *info, void *vinstance,
 376                         void *recvCallBackParam)
 377 {
 378
 379         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 380         switch (info->status) {
 381                 case NEW_DATA:
 382                         set_pwr_alert(orte_data);
 383                         break;
 384                 case DEADLINE:
 385                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 386                         break;
 387         }
 388 }
 389
 390 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 391                                 void *recvCallBackParam)
 392 {
 393         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 394
 395         switch (info->status) {
 396                 case NEW_DATA:
 397                         /* reversing motion direction, as it is different, than last year */
 398                         orte_data->motion_speed.left *= -1;
 399                         orte_data->motion_speed.right *=-1;
 400                         set_motor_speed(orte_data);
 401                         /*printf("motor cmd received\n");*/
 402                         break;
 403                 case DEADLINE:
 404                         printf("motor cmd deadline occurred, stopping motors\n");
 405                         orte_data->motion_speed.left = 0;
 406                         orte_data->motion_speed.right = 0;
 407                         set_motor_speed(orte_data);
 408                         break;
 409         }
 410 }
 411
 412 void rcv_lift_cb (const ORTERecvInfo *info, void *vinstance,
 413                         void *recvCallBackParam)
 414 {
 415         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 416
 417         switch (info->status) {
 418                 case NEW_DATA:
 419                         set_lift(orte_data);
 420                         break;
 421                 case DEADLINE:
 422 //                      printf("ORTE deadline occurred - lift receive\n");
 423                         break;
 424         }
 425 }
 426
 427 void rcv_chelae_cb (const ORTERecvInfo *info, void *vinstance,
 428                         void *recvCallBackParam)
 429 {
 430         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 431
 432         switch (info->status) {
 433                 case NEW_DATA:
 434                         set_chelae(orte_data);
 435                         break;
 436                 case DEADLINE:
 437 //                      printf("ORTE deadline occurred - chelae receive\n");
 438                         break;
 439         }
 440 }
 441
 442 void rcv_belts_cb (const ORTERecvInfo *info, void *vinstance,
 443                         void *recvCallBackParam)
 444 {
 445         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 446
 447         switch (info->status) {
 448                 case NEW_DATA:
 449                         set_belts(orte_data);
 450                         break;
 451                 case DEADLINE:
 452 //                      printf("ORTE deadline occurred - belts receive\n");
 453                         break;
 454         }
 455 }
 456
 457 void rcv_holder_cb (const ORTERecvInfo *info, void *vinstance,
 458                         void *recvCallBackParam)
 459 {
 460         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 461
 462         switch (info->status) {
 463                 case NEW_DATA:
 464                         set_holder(orte_data);
 465                         break;
 466                 case DEADLINE:
 467 //                      printf("ORTE deadline occurred - holder receive\n");
 468                         break;
 469         }
 470 }
 471
 472 void rcv_pusher_cb (const ORTERecvInfo *info, void *vinstance,
 473                         void *recvCallBackParam)
 474 {
 475         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 476
 477         switch (info->status) {
 478                 case NEW_DATA:
 479                         set_pusher(orte_data);
 480                         break;
 481                 case DEADLINE:
 482 //                      printf("ORTE deadline occurred - pusher receive\n");
 483                         break;
 484         }
 485 }
 486
 487 void rcv_hokuyo_pitch_cb (const ORTERecvInfo *info, void *vinstance,
 488                         void *recvCallBackParam)
 489 {
 490         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 491
 492         switch (info->status) {
 493                 case NEW_DATA:
 494                         set_hokuyo_pitch(orte_data);
 495                         break;
 496                 case DEADLINE:
 497 //                      printf("ORTE deadline occurred - hokuyo pitch receive\n");
 498                         break;
 499         }
 500 }
 501
 502
 503 int main(int argc, char *argv[])
 504 {
 505         fd_set read_fd_set;
 506         struct timeval timeout;
 507         int ret;
 508         int size;
 509         int rv = 0;
 510
 511         struct robottype_orte_data orte;
 512         struct can_frame frame;
 513
 514         if (cand_init() < 0) {
 515                 printf("cand: init failed\n");
 516                 exit(1);
 517         } else {
 518                 printf("cand: init OK\n");
 519         }
 520
 521         orte.strength = 1;
 522
 523         /* orte initialization */
 524         if(robottype_roboorte_init(&orte)) {
 525                 printf("Roboorte initialization failed! Exiting...\n");
 526                 exit(-1);
 527         }
 528         else
 529                 printf("Roboorte OK\n");
 530
 531         /* creating publishers */
 532         robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
 533         robottype_publisher_motion_status_create(&orte, NULL, NULL);
 534         robottype_publisher_motion_irc_create(&orte, NULL, NULL);
 535         robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
 536         robottype_publisher_cmu_create(&orte, NULL, NULL);
 537         robottype_publisher_corr_trig_create(&orte, NULL, NULL);
 538         robottype_publisher_corr_distances_create(&orte, NULL, NULL);
 539         robottype_publisher_puck_distance_create(&orte, NULL, NULL);
 540         printf("Publishers OK\n");
 541
 542         /* creating subscribers */
 543         robottype_subscriber_pwr_ctrl_create(&orte, rcv_pwr_ctrl_cb, &orte);
 544         robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
 545         robottype_subscriber_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
 546         robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
 547         robottype_subscriber_lift_create(&orte, rcv_lift_cb, &orte);
 548         robottype_subscriber_chelae_create(&orte, rcv_chelae_cb, &orte);
 549         robottype_subscriber_belts_create(&orte, rcv_belts_cb, &orte);
 550         robottype_subscriber_holder_create(&orte, rcv_holder_cb, &orte);
 551         robottype_subscriber_pusher_create(&orte, rcv_pusher_cb, &orte);
 552         robottype_subscriber_hokuyo_pitch_create(&orte, rcv_hokuyo_pitch_cb, &orte);
 553
 554         printf("subscribers OK\n");
 555
 556         /* main loop */
 557         for(;;) {
 558                 FD_ZERO(&read_fd_set);
 559                 FD_SET(sock, &read_fd_set);
 560                 timeout.tv_sec = 0;
 561                 timeout.tv_usec = 10000;
 562
 563                 ret = select(FD_SETSIZE, &read_fd_set, NULL, NULL, &timeout);
 564                 if (ret < 0) {
 565                         perror("cand: select() error");
 566                         goto err;
 567                 }
 568
 569                 /* timeout expired, nothing to be done */
 570                 if (ret == 0)
 571                         continue;
 572
 573                 if (!FD_ISSET(sock, &read_fd_set))
 574                         continue;
 575
 576                 /* read data from SocketCAN */
 577                 size = read(sock, &frame, sizeof(struct can_frame));
 578
 579                 /* parse data */
 580                 cand_parse_frame(&orte, frame);
 581         }
 582
 583 err:
 584         close(sock);
 585         return 1;
 586 out:
 587         return EXIT_SUCCESS;
 588 }