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)|
 288                                          (frame.data[1]<<16)|
 289                                          (frame.data[2]<<8));
 290                         orte->motion_irc.left =
 291                                         ((frame.data[3]<<24)|
 292                                          (frame.data[4]<<16)|
 293                                          (frame.data[5]<<8));
 294                         orte->motion_irc.seq = frame.data[6];
 295                         ORTEPublicationSend(orte->publication_motion_irc);
 296                         break;
 297
 298                 /* motion status */
 299                 case CAN_MOTION_STATUS:
 300                         orte->motion_status.err_left =
 301                                         (frame.data[0]<<8)|(frame.data[1]);
 302                         orte->motion_status.err_right =
 303                                         (frame.data[2]<<8)|(frame.data[3]);
 304                         if(++status_cnt == 5) {
 305                                 ORTEPublicationSend(orte->publication_motion_status);
 306                                 status_cnt = 0;
 307                         }
 308                         break;
 309                 case CAN_PWR_ADC1:
 310                         double volt33, voltBAT;
 311                         voltBAT = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 312                                  (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 313                         volt33 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 314                                  (frame.data[6] << 8) | (frame.data[7]))/10000.0;
 315                         orte->pwr_voltage.voltage33 = volt33;
 316                         orte->pwr_voltage.voltageBAT = voltBAT;
 317                         break;
 318
 319                 case CAN_PWR_ADC2:
 320                         double volt80, volt50;
 321                         volt50 = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 322                                  (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 323                         volt80 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 324                                  (frame.data[6] << 8) | (frame.data[7]))/10000.0;
 325                         orte->pwr_voltage.voltage80 = volt80;
 326                         orte->pwr_voltage.voltage50 = volt50;
 327
 328                         ORTEPublicationSend(orte->publication_pwr_voltage);
 329
 330                         break;
 331                 /* data from the sharp sensor measuring puck to sensor distance */
 332                 case CAN_ADC_1: /* data from the sharp sensor measuring distance of the puck (column element) */
 333                         orte->puck_distance.distance = puckSharp_ir2mm(frame.data[0]<<8 | frame.data[1])/1000.0;
 334                         ORTEPublicationSend(orte->publication_puck_distance);
 335                         break;
 336                 /* lift & pusher status, their position and status bit */
 337                 case CAN_LP_STATUS:
 338                         /* printf("lift position: %d \n", frame.data[1]<<8 | frame.data[2]); */
 339                         orte->actuator_status.pusher_pos = frame.data[0]<<8 | frame.data[1];
 340                         orte->actuator_status.lift_pos   = frame.data[2]<<8 | frame.data[3];
 341                         orte->actuator_status.status     = frame.data[4];
 342                         ORTEPublicationSend(orte->publication_actuator_status);
 343                         break;
 344                 /* laser data */
 345                 case CAN_CMU:
 346                         orte->cmu.color = frame.data[0];
 347                         ORTEPublicationSend(orte->publication_cmu);
 348                         break;
 349
 350                 default:
 351 //                      printf("received CAN msg with unknown id: %x\n",frame.can_id);
 352                         break;
 353         }
 354 }
 355
 356 void rcv_pwr_ctrl_cb (const ORTERecvInfo *info, void *vinstance,
 357                         void *recvCallBackParam)
 358 {
 359         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 360
 361         switch (info->status) {
 362                 case NEW_DATA:
 363                         set_pwr_ctrl(orte_data);
 364                         break;
 365                 case DEADLINE:
 366                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 367                         break;
 368         }
 369 }
 370
 371 void rcv_can_msg_cb (const ORTERecvInfo *info, void *vinstance,
 372                         void *recvCallBackParam)
 373 {
 374         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 375
 376         switch (info->status) {
 377                 case NEW_DATA:
 378                         send_can_msg(orte_data);
 379                         break;
 380                 case DEADLINE:
 381                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 382                         break;
 383         }
 384 }
 385 void rcv_pwr_alert_cb (const ORTERecvInfo *info, void *vinstance,
 386                         void *recvCallBackParam)
 387 {
 388
 389         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 390         switch (info->status) {
 391                 case NEW_DATA:
 392                         set_pwr_alert(orte_data);
 393                         break;
 394                 case DEADLINE:
 395                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 396                         break;
 397         }
 398 }
 399
 400 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 401                                 void *recvCallBackParam)
 402 {
 403         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 404
 405         switch (info->status) {
 406                 case NEW_DATA:
 407                         /* reversing motion direction, as it is different, than last year */
 408                         orte_data->motion_speed.left *= -1;
 409                         orte_data->motion_speed.right *=-1;
 410                         set_motor_speed(orte_data);
 411                         /*printf("motor cmd received\n");*/
 412                         break;
 413                 case DEADLINE:
 414                         //printf("motor cmd deadline occurred, stopping motors\n");
 415                         orte_data->motion_speed.left = 0;
 416                         orte_data->motion_speed.right = 0;
 417                         set_motor_speed(orte_data);
 418                         break;
 419         }
 420 }
 421
 422 void rcv_lift_cb (const ORTERecvInfo *info, void *vinstance,
 423                         void *recvCallBackParam)
 424 {
 425         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 426
 427         switch (info->status) {
 428                 case NEW_DATA:
 429                         set_lift(orte_data);
 430                         break;
 431                 case DEADLINE:
 432 //                      printf("ORTE deadline occurred - lift receive\n");
 433                         break;
 434         }
 435 }
 436
 437 void rcv_chelae_cb (const ORTERecvInfo *info, void *vinstance,
 438                         void *recvCallBackParam)
 439 {
 440         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 441
 442         switch (info->status) {
 443                 case NEW_DATA:
 444                         set_chelae(orte_data);
 445                         break;
 446                 case DEADLINE:
 447 //                      printf("ORTE deadline occurred - chelae receive\n");
 448                         break;
 449         }
 450 }
 451
 452 void rcv_belts_cb (const ORTERecvInfo *info, void *vinstance,
 453                         void *recvCallBackParam)
 454 {
 455         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 456
 457         switch (info->status) {
 458                 case NEW_DATA:
 459                         set_belts(orte_data);
 460                         break;
 461                 case DEADLINE:
 462 //                      printf("ORTE deadline occurred - belts receive\n");
 463                         break;
 464         }
 465 }
 466
 467 void rcv_holder_cb (const ORTERecvInfo *info, void *vinstance,
 468                         void *recvCallBackParam)
 469 {
 470         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 471
 472         switch (info->status) {
 473                 case NEW_DATA:
 474                         set_holder(orte_data);
 475                         break;
 476                 case DEADLINE:
 477 //                      printf("ORTE deadline occurred - holder receive\n");
 478                         break;
 479         }
 480 }
 481
 482 void rcv_pusher_cb (const ORTERecvInfo *info, void *vinstance,
 483                         void *recvCallBackParam)
 484 {
 485         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 486
 487         switch (info->status) {
 488                 case NEW_DATA:
 489                         set_pusher(orte_data);
 490                         break;
 491                 case DEADLINE:
 492 //                      printf("ORTE deadline occurred - pusher receive\n");
 493                         break;
 494         }
 495 }
 496
 497 void rcv_hokuyo_pitch_cb (const ORTERecvInfo *info, void *vinstance,
 498                         void *recvCallBackParam)
 499 {
 500         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 501
 502         switch (info->status) {
 503                 case NEW_DATA:
 504                         set_hokuyo_pitch(orte_data);
 505                         break;
 506                 case DEADLINE:
 507 //                      printf("ORTE deadline occurred - hokuyo pitch receive\n");
 508                         break;
 509         }
 510 }
 511
 512
 513 int main(int argc, char *argv[])
 514 {
 515         fd_set read_fd_set;
 516         struct timeval timeout;
 517         int ret;
 518         int size;
 519         int rv = 0;
 520
 521         struct robottype_orte_data orte;
 522         struct can_frame frame;
 523
 524         if (cand_init() < 0) {
 525                 printf("cand: init failed\n");
 526                 exit(1);
 527         } else {
 528                 printf("cand: init OK\n");
 529         }
 530
 531         orte.strength = 1;
 532
 533         /* orte initialization */
 534         if(robottype_roboorte_init(&orte)) {
 535                 printf("Roboorte initialization failed! Exiting...\n");
 536                 exit(-1);
 537         }
 538         else
 539                 printf("Roboorte OK\n");
 540
 541         /* creating publishers */
 542         robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
 543         robottype_publisher_motion_status_create(&orte, NULL, NULL);
 544         robottype_publisher_motion_irc_create(&orte, NULL, NULL);
 545         robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
 546         robottype_publisher_cmu_create(&orte, NULL, NULL);
 547         robottype_publisher_corr_trig_create(&orte, NULL, NULL);
 548         robottype_publisher_corr_distances_create(&orte, NULL, NULL);
 549         robottype_publisher_puck_distance_create(&orte, NULL, NULL);
 550         robottype_publisher_actuator_status_create(&orte, NULL, NULL);
 551         printf("Publishers OK\n");
 552
 553         /* creating subscribers */
 554         robottype_subscriber_pwr_ctrl_create(&orte, rcv_pwr_ctrl_cb, &orte);
 555         robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
 556         robottype_subscriber_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
 557         robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
 558         robottype_subscriber_lift_create(&orte, rcv_lift_cb, &orte);
 559         robottype_subscriber_chelae_create(&orte, rcv_chelae_cb, &orte);
 560         robottype_subscriber_belts_create(&orte, rcv_belts_cb, &orte);
 561         robottype_subscriber_holder_create(&orte, rcv_holder_cb, &orte);
 562         robottype_subscriber_pusher_create(&orte, rcv_pusher_cb, &orte);
 563         robottype_subscriber_hokuyo_pitch_create(&orte, rcv_hokuyo_pitch_cb, &orte);
 564
 565         printf("subscribers OK\n");
 566
 567         /* main loop */
 568         for(;;) {
 569                 FD_ZERO(&read_fd_set);
 570                 FD_SET(sock, &read_fd_set);
 571                 timeout.tv_sec = 0;
 572                 timeout.tv_usec = 10000;
 573
 574                 ret = select(FD_SETSIZE, &read_fd_set, NULL, NULL, &timeout);
 575                 if (ret < 0) {
 576                         perror("cand: select() error");
 577                         goto err;
 578                 }
 579
 580                 /* timeout expired, nothing to be done */
 581                 if (ret == 0)
 582                         continue;
 583
 584                 if (!FD_ISSET(sock, &read_fd_set))
 585                         continue;
 586
 587                 /* read data from SocketCAN */
 588                 size = read(sock, &frame, sizeof(struct can_frame));
 589
 590                 /* parse data */
 591                 cand_parse_frame(&orte, frame);
 592         }
 593
 594 err:
 595         close(sock);
 596         return 1;
 597 out:
 598         return EXIT_SUCCESS;
 599 }