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         /*printf("data: ");
 126         for (int i=0; i< 4; i++) {
 127                 printf("%02x ",data[i]);
 128         }
 129         printf("\n");*/
 130
 131         return 0;
 132 }
 133
 134 /**
 135  * Sends #CAN_LIFT message.
 136  *
 137  * - data[1] = orte_data->lift.pos & 0xFF; // lower byte
 138  * - data[0] = orte_data->lift.pos >> 8; // upper byte
 139  */
 140 int set_lift(struct robottype_orte_data *orte_data)
 141 {
 142         unsigned char data[2];
 143
 144         data[0] = orte_data->lift.pos >> 8;
 145         data[1] = orte_data->lift.pos & 0xFF;
 146         can_send(CAN_LIFT, sizeof(data), data);
 147
 148         return 0;
 149 }
 150
 151 /**
 152  * Sends #CAN_SERVO message.
 153  *
 154  * - data[1] = orte_data->chelas.left;
 155  * - data[0] = orte_data->chelas.right;
 156  */
 157 int set_chelas(struct robottype_orte_data *orte_data)
 158 {
 159         unsigned char data[2];
 160
 161         data[0] = orte_data->chelas.left;
 162         data[1] = orte_data->chelas.right;
 163         can_send(CAN_SERVO, sizeof(data), data);
 164
 165         return 0;
 166 }
 167
 168 /**
 169  * Sends #CAN_DRIVES message.
 170  *
 171  * - data[1] = orte_data->belts.left;
 172  * - data[0] = orte_data->belts.right;
 173  */
 174 int set_belts(struct robottype_orte_data *orte_data)
 175 {
 176         unsigned char data[2];
 177
 178         data[0] = orte_data->belts.left;
 179         data[1] = orte_data->belts.right;
 180         can_send(CAN_DRIVES, sizeof(data), data);
 181
 182         return 0;
 183 }
 184
 185 /**
 186  * Sends #CAN_HOLDER message.
 187  *
 188  * - data[0] = orte_data->holder.pos;
 189  */
 190 int set_holder(struct robottype_orte_data *orte_data)
 191 {
 192         unsigned char data[2];
 193
 194         data[0] = orte_data->holder.pos;
 195         can_send(CAN_HOLDER, sizeof(data), data);
 196
 197         return 0;
 198 }
 199
 200 /**
 201  * Sends #CAN_PUSHER message.
 202  *
 203  * - data[1] = orte_data->pusher.pos & 0xFF; // lower byte
 204  * - data[0] = orte_data->pusher.pos >> 8; // upper byte
 205  */
 206 int set_pusher(struct robottype_orte_data *orte_data)
 207 {
 208         unsigned char data[2];
 209
 210         data[0] = orte_data->pusher.pos >> 8;
 211         data[1] = orte_data->pusher.pos & 0xFF;
 212         can_send(CAN_PUSHER, sizeof(data), data);
 213
 214         return 0;
 215 }
 216
 217
 218 int can_send(canid_t id, unsigned char length, unsigned char *data)
 219 {
 220         struct can_frame frame;
 221         int size;
 222
 223         frame.can_id = id;
 224         frame.can_dlc = length;
 225
 226         for(int i=0; i<length; i++)
 227                 frame.data[i] = data[i];
 228
 229         if ((size = write(sock, &frame, sizeof(struct can_frame))) < 0) {
 230                 perror("cand: can send: write()");
 231                 return -1;
 232         } else if (size < sizeof(struct can_frame)) {
 233                 perror("cand: can send: incomplete CAN frame\n");
 234                 return 1;
 235         }
 236
 237         /*
 238         printf("write returned %d\n", size);
 239         printf("finnished can send\n");
 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                 //      printf("PWR_ADC1: 3,3V = %2.2f, BAT = %2.2f\n",volt33,voltBAT);
 313                         orte->pwr_voltage.voltage33 = volt33;
 314                         orte->pwr_voltage.voltageBAT = voltBAT;
 315                         break;
 316
 317                 case CAN_PWR_ADC2:
 318                         double volt80, volt50;
 319                         volt50 = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 320                                  (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 321                         volt80 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 322                                  (frame.data[6] << 8) | (frame.data[7]))/10000.0;
 323                 //      printf("PWR_ADC2: 8,0V = %2.2f, 5,0V = %2.2f\n",volt80,volt50);
 324                         orte->pwr_voltage.voltage80 = volt80;
 325                         orte->pwr_voltage.voltage50 = volt50;
 326
 327                         ORTEPublicationSend(orte->publication_pwr_voltage);
 328
 329                         break;
 330                 case CAN_ADC_1:
 331                 //      ORTEPublicationSend(orte->publication_sharps);
 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_chelas_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_chelas(orte_data);
 435                         break;
 436                 case DEADLINE:
 437 //                      printf("ORTE deadline occurred - chelas 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 int main(int argc, char *argv[])
 488 {
 489         fd_set read_fd_set;
 490         struct timeval timeout;
 491         int ret;
 492         int size;
 493         int rv = 0;
 494
 495         struct robottype_orte_data orte;
 496         struct can_frame frame;
 497
 498         if (cand_init() < 0) {
 499                 printf("cand: init failed\n");
 500                 exit(1);
 501         } else {
 502                 printf("cand: init OK\n");
 503         }
 504
 505         orte.strength = 1;
 506
 507         /* orte initialization */
 508         if(robottype_roboorte_init(&orte)) {
 509                 printf("Roboorte initialization failed! Exiting...\n");
 510                 exit(-1);
 511         }
 512
 513         /* creating publishers */
 514         robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
 515         robottype_publisher_motion_status_create(&orte, NULL, NULL);
 516         robottype_publisher_motion_irc_create(&orte, NULL, NULL);
 517         robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
 518         robottype_publisher_cmu_create(&orte, NULL, NULL);
 519
 520         /* creating subscribers */
 521         robottype_subscriber_pwr_ctrl_create(&orte, rcv_pwr_ctrl_cb, &orte);
 522         robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
 523         robottype_subscriber_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
 524         robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
 525         robottype_subscriber_lift_create(&orte, rcv_lift_cb, &orte);
 526         robottype_subscriber_chelas_create(&orte, rcv_lift_cb, &orte);
 527         robottype_subscriber_belts_create(&orte, rcv_belts_cb, &orte);
 528         robottype_subscriber_holder_create(&orte, rcv_holder_cb, &orte);
 529         robottype_subscriber_pusher_create(&orte, rcv_pusher_cb, &orte);
 530
 531         /* main loop */
 532         for(;;) {
 533                 FD_ZERO(&read_fd_set);
 534                 FD_SET(sock, &read_fd_set);
 535                 timeout.tv_sec = 0;
 536                 timeout.tv_usec = 10000;
 537
 538                 ret = select(FD_SETSIZE, &read_fd_set, NULL, NULL, &timeout);
 539                 if (ret < 0) {
 540                         perror("cand: select() error");
 541                         goto err;
 542                 }
 543
 544                 /* timeout expired, nothing to be done */
 545                 if (ret == 0)
 546                         continue;
 547
 548                 if (!FD_ISSET(sock, &read_fd_set))
 549                         continue;
 550
 551                 /* read data from SocketCAN */
 552                 size = read(sock, &frame, sizeof(struct can_frame));
 553
 554                 /* parse data */
 555                 cand_parse_frame(&orte, frame);
 556         }
 557
 558 err:
 559         close(sock);
 560         return 1;
 561 out:
 562         return EXIT_SUCCESS;
 563 }