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