src/cand/cand.cc

   1 /*
   2  * cand_eb2008.cc                       08/04/08
   3  *
   4  * CAN daemon for the Eurobot 2008 competition.
   5  *
   6  * Copyright: (c) 2008 CTU Dragons
   7  *            CTU FEE - Department of Control Engineering
   8  * License: GNU GPL v.2
   9  */
  10
  11 #ifdef HAVE_CONFIG_H
  12 #include <config.h>
  13 #endif
  14
  15 #include <cstring>
  16 #include <iostream>
  17 #include <cstdlib>
  18 #include <sys/socket.h>
  19 #include <sys/ioctl.h>
  20 #include <fcntl.h>
  21
  22 #include <af_can.h>
  23 #include <canutils.h>
  24
  25 #include <can_ids.h>
  26 #include <can_msg_masks.h>
  27 #include <can_msg_def.h>
  28 #include <sharp.h>
  29 #include <orte.h>
  30 #include <roboorte.h>
  31 #include <roboorte_robottype.h>
  32 #include <can_msg_masks.h>
  33 #include "cand.h"
  34
  35 int cand_init()
  36 {
  37         if ((sock = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
  38                 perror("cand: unable to create a socket");
  39                 return -1;
  40         }
  41
  42         can_addr.can_family = AF_CAN;
  43         std::strcpy(can_ifreq.ifr_name, "can0");
  44
  45         if (ioctl(sock, SIOCGIFINDEX, &can_ifreq) < 0) {
  46                 perror("cand: SIOCGIFINDEX");
  47                 return -2;
  48         }
  49
  50         can_addr.can_ifindex = can_ifreq.ifr_ifindex;
  51
  52         if (!can_addr.can_ifindex) {
  53                 perror("cand: invalid socket interface");
  54                 return -3;
  55         }
  56
  57         if (bind(sock, (struct sockaddr *)&can_addr, sizeof(can_addr)) < 0) {
  58                 perror("cand: unable to bind");
  59                 return -4;
  60         }
  61
  62         return 0;
  63 }
  64
  65 int set_pwr_ctrl(struct robottype_orte_data *orte_data)
  66 {
  67         unsigned char data=0;
  68         if(orte_data->pwr_ctrl.voltage33)
  69                 data |= CAN_PWR_CTRL_33_ON;
  70         else
  71                 data |= CAN_PWR_CTRL_33_OFF;
  72
  73         if(orte_data->pwr_ctrl.voltage50)
  74                 data |= CAN_PWR_CTRL_50_ON;
  75         else
  76                 data |= CAN_PWR_CTRL_50_OFF;
  77
  78         if(orte_data->pwr_ctrl.voltage80)
  79                 data |= CAN_PWR_CTRL_80_ON;
  80         else
  81                 data |= CAN_PWR_CTRL_80_OFF;
  82
  83         can_send(CAN_PWR, 1, &data);
  84
  85         return 0;
  86 }
  87
  88 int set_pwr_alert(struct robottype_orte_data *orte_data)
  89 {
  90         unsigned char data=0;
  91         data = orte_data->pwr_alert.value;
  92
  93         can_send(CAN_PWR_ALERT, 1, &data);
  94
  95         return 0;
  96 }
  97
  98 int send_can_msg(struct robottype_orte_data *orte_data)
  99 {
 100         uint8_t data[8];
 101
 102         data[0] = orte_data->can_msg.data1;
 103         data[1] = orte_data->can_msg.data2;
 104         data[2] = orte_data->can_msg.data3;
 105         data[3] = orte_data->can_msg.data4;
 106         data[4] = orte_data->can_msg.data5;
 107         data[5] = orte_data->can_msg.data6;
 108         data[6] = orte_data->can_msg.data7;
 109         data[7] = orte_data->can_msg.data8;
 110
 111         can_send(orte_data->can_msg.id, orte_data->can_msg.len, data);
 112 }
 113
 114 int set_motor_speed(struct robottype_orte_data *orte_data)
 115 {
 116         unsigned char data[4];
 117
 118         data[0] = orte_data->motion_speed.right >> 8;
 119         data[1] = orte_data->motion_speed.right & 0xff;
 120         data[2] = orte_data->motion_speed.left >> 8;
 121         data[3] = orte_data->motion_speed.left & 0xff;
 122         can_send(CAN_MOTION_CMD, 4, data);
 123
 124         /*printf("data: ");
 125         for (int i=0; i< 4; i++) {
 126                 printf("%02x ",data[i]);
 127         }
 128         printf("\n");*/
 129
 130         return 0;
 131 }
 132
 133 int set_serva(struct robottype_orte_data *orte_data)
 134 {
 135         unsigned char data[4];
 136
 137         data[3] = orte_data->servos.reserve;
 138         data[2] = orte_data->servos.holder;
 139         data[1] = orte_data->servos.brush_right;
 140         data[0] = orte_data->servos.brush_left;
 141         can_send(CAN_SERVO, 4, data);
 142
 143         return 0;
 144 }
 145
 146 int set_laser_cmd(struct robottype_orte_data *orte_data)
 147 {
 148         unsigned char data[1];
 149
 150         data[0] = orte_data->laser_cmd.speed;
 151         can_send(CAN_LAS_CMD, 1, data);
 152
 153         return 0;
 154 }
 155
 156 int set_drives(struct robottype_orte_data *orte_data)
 157 {
 158         unsigned char data[2];
 159
 160         data[1] = orte_data->drives.brush_right;
 161         data[0] = orte_data->drives.brush_left;
 162         can_send(CAN_DRIVES, 2, data);
 163
 164         return 0;
 165 }
 166 int can_send(canid_t id, unsigned char length, unsigned char *data)
 167 {
 168         struct can_frame frame;
 169         int size;
 170
 171         frame.can_id = id;
 172         frame.can_dlc = length;
 173
 174         for(int i=0; i<length; i++)
 175                 frame.data[i] = data[i];
 176
 177         if ((size = write(sock, &frame, sizeof(struct can_frame))) < 0) {
 178                 perror("cand: can send: write()");
 179                 return -1;
 180         } else if (size < sizeof(struct can_frame)) {
 181                 perror("cand: can send: incomplete CAN frame\n");
 182                 return 1;
 183         }
 184
 185         /*
 186         printf("write returned %d\n", size);
 187         printf("finnished can send\n");
 188         */
 189
 190         return 0;
 191 }
 192
 193 /**
 194  * Parse frame ID and react as required
 195  */
 196 int cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
 197 {
 198         int status_cnt = 0;
 199         int adc1_cnt = 0;
 200         int adc2_cnt = 0;
 201         int adc3_cnt = 0;
 202         int ir_cnt = 0;
 203         unsigned short *las_meas;
 204         int las_mi, las_di, las_bcnt;
 205         static unsigned short prev_meas;
 206         int i;
 207
 208         switch(frame.can_id) {
 209                 /* voltage measurements from power board */
 210
 211                 /* robot commands (start, ..) */
 212                 case CAN_ROBOT_CMD:
 213                         orte->robot_cmd.start = frame.data[0];
 214                         orte->robot_cmd.stop = frame.data[1];
 215                         ORTEPublicationSend(orte->publication_robot_cmd);
 216                         break;
 217
 218                 /* positioning by odometry */
 219                 case CAN_MOTION_ODOMETRY_SIMPLE:
 220                         orte->motion_irc.right =
 221                                         ((frame.data[0]<<24)|(frame.data[1]<<16)|
 222                                         (frame.data[2]<<8)|(frame.data[3]));
 223                         orte->motion_irc.left =
 224                                         ((frame.data[4]<<24)|(frame.data[5]<<16)|
 225                                          (frame.data[6]<<8)|(frame.data[7]));
 226                         ORTEPublicationSend(orte->publication_motion_irc);
 227                         break;
 228
 229                 /* motion status */
 230                 case CAN_MOTION_STATUS:
 231                         orte->motion_status.err_left =
 232                                         (frame.data[0]<<8)|(frame.data[1]);
 233                         orte->motion_status.err_right =
 234                                         (frame.data[2]<<8)|(frame.data[3]);
 235                         if(++status_cnt == 5) {
 236                                 ORTEPublicationSend(orte->publication_motion_status);
 237                                 status_cnt = 0;
 238                         }
 239                         break;
 240                 case CAN_PWR_ADC1:
 241                         double volt33, voltBAT;
 242                         voltBAT = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 243                                  (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 244                         volt33 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 245                                  (frame.data[6] << 8) | (frame.data[7]))/10000.0;
 246                 //      printf("PWR_ADC1: 3,3V = %2.2f, BAT = %2.2f\n",volt33,voltBAT);
 247                         orte->pwr_voltage.voltage33 = volt33;
 248                         orte->pwr_voltage.voltageBAT = voltBAT;
 249                         break;
 250
 251                 case CAN_PWR_ADC2:
 252                         double volt80, volt50;
 253                         volt50 = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
 254                                  (frame.data[2] << 8) | (frame.data[3]))/10000.0;
 255                         volt80 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
 256                                  (frame.data[6] << 8) | (frame.data[7]))/10000.0;
 257                 //      printf("PWR_ADC2: 8,0V = %2.2f, 5,0V = %2.2f\n",volt80,volt50);
 258                         orte->pwr_voltage.voltage80 = volt80;
 259                         orte->pwr_voltage.voltage50 = volt50;
 260
 261                         ORTEPublicationSend(orte->publication_pwr_voltage);
 262
 263                         break;
 264                 case CAN_ADC_1:
 265                         orte->sharps.front_right = s_gp2y0a21_ir2mmLong((frame.data[0] << 8)| \
 266                                                           (frame.data[1]))/1000.0;
 267                         orte->sharps.right = s_gp2y0a21_ir2mmLong((frame.data[2] << 8)| \
 268                                                           (frame.data[3]))/1000.0;
 269                         orte->sharps.left = s_gp2y0a21_ir2mmLong((frame.data[4] << 8)| \
 270                                                           (frame.data[5]))/1000.0;
 271                         orte->sharps.front_left = s_gp2y0a21_ir2mmLong((frame.data[6] << 8)| \
 272                                                           (frame.data[7]))/1000.0;
 273                         ORTEPublicationSend(orte->publication_sharps);
 274                         break;
 275
 276                 case CAN_ADC_2:
 277                         orte->sharps.back_left = s_gp2d120_ir2mmShort((frame.data[4] << 8)| \
 278                                                           (frame.data[5]))/1000.0;
 279                         orte->sharps.back_right = s_gp2d120_ir2mmShort((frame.data[6] << 8)| \
 280                                                           (frame.data[7]))/1000.0;
 281                         ORTEPublicationSend(orte->publication_sharps);
 282                         break;
 283                 /* laser data */
 284                 case CAN_LAS1:
 285 //                      printf("CAN: ");
 286 //                      for (i=0; i<frame.can_dlc; i++) {
 287 //                              printf("0x%02x ", frame.data[i]);
 288 //                      }
 289 //                      printf("can_dlc=%d\n", frame.can_dlc);
 290                         orte->laser_meas.cnt = frame.data[1];
 291                         las_bcnt = orte->laser_meas.cnt;
 292                         last_id = frame.data[0];
 293                         las_di = 4;
 294                         /* rotation period */
 295                         orte->laser_meas.period = (frame.data[2]<<8)|(frame.data[3]);
 296 //                      printf("CAN ID=0x%02x: cnt=%d period=%d measures: ",
 297 //                                      frame.can_id, orte->laser_meas.cnt, orte->laser_meas.period);
 298
 299                         for (las_mi=0; las_mi<(frame.can_dlc-4)/2; las_mi++) {
 300                                 switch (las_mi) {
 301                                         case 0: las_meas = &orte->laser_meas.measures0; break;
 302                                         case 1: las_meas = &orte->laser_meas.measures1; break;
 303                                         default: break;
 304                                 }
 305                                 *las_meas =
 306                                         (frame.data[las_di++]<<8)|(frame.data[las_di++]);
 307 //                              printf("0x%02x 0x%02x %u ",
 308 //                                      frame.data[las_di-2], frame.data[las_di-1], *las_meas);
 309                         }
 310 //                      printf("\n");
 311                         break;
 312                 case CAN_LAS2:
 313                 case CAN_LAS3:
 314                 case CAN_LAS4:
 315 //                      printf("CAN: ");
 316 //                      for (i=0; i<frame.can_dlc; i++) {
 317 //                              printf("0x%02x ", frame.data[i]);
 318 //                      }
 319 //                      printf("can_dlc=%d\n", frame.can_dlc);
 320 //                      printf("CAN ID=0x%02x: cnt=%d period=%d measures: ",
 321 //                              frame.can_id, orte->laser_meas.cnt, orte->laser_meas.period);
 322                         if (frame.data[0] != (last_id+(frame.can_id-CAN_LAS1)))
 323                                 break;
 324                         las_di = 2;
 325                         while (las_di < 8 && las_bcnt > 0) {
 326                                 switch (las_mi) {
 327                                         case 2: las_meas = &orte->laser_meas.measures2; break;
 328                                         case 3: las_meas = &orte->laser_meas.measures3; break;
 329                                         case 4: las_meas = &orte->laser_meas.measures4; break;
 330                                         case 5: las_meas = &orte->laser_meas.measures5; break;
 331                                         case 6: las_meas = &orte->laser_meas.measures6; break;
 332                                         case 7: las_meas = &orte->laser_meas.measures7; break;
 333                                         case 8: las_meas = &orte->laser_meas.measures8; break;
 334                                         case 9: las_meas = &orte->laser_meas.measures9; break;
 335                                         default: break;
 336                                 }
 337                                 *las_meas =
 338                                         (frame.data[las_di++]<<8)|(frame.data[las_di++]);
 339 //                              printf("0x%02x 0x%02x %u ",
 340 //                                      frame.data[las_di-2], frame.data[las_di-1], *las_meas);
 341                                 las_mi++;
 342                                 las_bcnt--;
 343                         }
 344                         printf("\n");
 345                         if (las_bcnt == 0) {
 346 //                              printf("ORTEPublicationSend()\n");
 347                                 ORTEPublicationSend(orte->publication_laser_meas);
 348                         }
 349                         break;
 350
 351                 case CAN_LAS_DATA:
 352                         orte->laser_data.period = (frame.data[0]<<8)|(frame.data[1]);
 353                         orte->laser_data.measure = (frame.data[2]<<8)|(frame.data[3]);
 354                         if (prev_meas != orte->laser_data.measure) {
 355                                 ORTEPublicationSend(orte->publication_laser_data);
 356                                 prev_meas = orte->laser_data.measure;
 357                         }
 358                         break;
 359
 360                 case CAN_CMU:
 361                         orte->cmu.color = frame.data[0];
 362                         ORTEPublicationSend(orte->publication_cmu);
 363                         break;
 364
 365                 case CAN_BUMPER:
 366                         orte->bumper.left = frame.data[0] & LEFT_BUMPER;
 367                         orte->bumper.right = frame.data[0] & RIGHT_BUMPER;
 368                         ORTEPublicationSend(orte->publication_bumper);
 369                         break;
 370
 371                 default:
 372 //                      printf("received CAN msg with unknown id: %x\n",frame.can_id);
 373                         break;
 374         }
 375 }
 376
 377 void rcv_pwr_ctrl_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                         set_pwr_ctrl(orte_data);
 385                         break;
 386                 case DEADLINE:
 387                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 388                         break;
 389         }
 390 }
 391
 392 void rcv_can_msg_cb (const ORTERecvInfo *info, void *vinstance,
 393                         void *recvCallBackParam)
 394 {
 395         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 396
 397         switch (info->status) {
 398                 case NEW_DATA:
 399                         send_can_msg(orte_data);
 400                         break;
 401                 case DEADLINE:
 402                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 403                         break;
 404         }
 405 }
 406 void rcv_pwr_alert_cb (const ORTERecvInfo *info, void *vinstance,
 407                         void *recvCallBackParam)
 408 {
 409
 410         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 411         switch (info->status) {
 412                 case NEW_DATA:
 413                         set_pwr_alert(orte_data);
 414                         break;
 415                 case DEADLINE:
 416                         //printf("ORTE deadline occurred - PWR_CTRL receive\n");
 417                         break;
 418         }
 419 }
 420
 421 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
 422                                 void *recvCallBackParam)
 423 {
 424         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 425
 426         switch (info->status) {
 427                 case NEW_DATA:
 428                         /* reversing motion direction, as it is different, than last year */
 429                         orte_data->motion_speed.left *= -1;
 430                         orte_data->motion_speed.right *=-1;
 431                         set_motor_speed(orte_data);
 432                         /*printf("motor cmd received\n");*/
 433                         break;
 434                 case DEADLINE:
 435                         printf("motor cmd deadline occurred, stopping motors\n");
 436                         orte_data->motion_speed.left = 0;
 437                         orte_data->motion_speed.right = 0;
 438                         set_motor_speed(orte_data);
 439                         break;
 440         }
 441 }
 442
 443 void rcv_servos_cb (const ORTERecvInfo *info, void *vinstance,
 444                         void *recvCallBackParam)
 445 {
 446         struct robottype_orte_data *orte_data =
 447                         (struct robottype_orte_data *)recvCallBackParam;
 448
 449         switch (info->status) {
 450                 case NEW_DATA:
 451                         set_serva(orte_data);
 452                         break;
 453                 case DEADLINE:
 454 //                      printf("ORTE deadline occurred - servo receive\n");
 455                         break;
 456         }
 457 }
 458
 459 void rcv_laser_cmd_cb (const ORTERecvInfo *info, void *vinstance,
 460                         void *recvCallBackParam)
 461 {
 462         struct robottype_orte_data *orte_data =
 463                         (struct robottype_orte_data *)recvCallBackParam;
 464
 465         switch (info->status) {
 466                 case NEW_DATA:
 467                         set_laser_cmd(orte_data);
 468                         break;
 469                 case DEADLINE:
 470 //                      printf("ORTE deadline occurred - servo receive\n");
 471                         break;
 472         }
 473 }
 474
 475 void rcv_drives_cb (const ORTERecvInfo *info, void *vinstance,
 476                         void *recvCallBackParam)
 477 {
 478         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 479
 480         switch (info->status) {
 481                 case NEW_DATA:
 482                         set_drives(orte_data);
 483                         break;
 484                 case DEADLINE:
 485 //                      printf("ORTE deadline occurred - drives receive\n");
 486                         break;
 487         }
 488 }
 489
 490 int main(int argc, char *argv[])
 491 {
 492         fd_set read_fd_set;
 493         struct timeval timeout;
 494         int ret;
 495         int size;
 496         int rv = 0;
 497
 498         struct robottype_orte_data orte;
 499         struct can_frame frame;
 500
 501         if (cand_init() < 0) {
 502                 printf("cand: init failed\n");
 503                 exit(1);
 504         } else {
 505                 printf("cand: init OK\n");
 506         }
 507
 508         orte.strength = 1;
 509
 510         /* orte initialization */
 511         robottype_roboorte_init(&orte);
 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_laser_meas_create(&orte, NULL, NULL);
 519         robottype_publisher_laser_data_create(&orte, NULL, NULL);
 520         robottype_publisher_sharps_create(&orte, NULL, NULL);
 521         robottype_publisher_cmu_create(&orte, NULL, NULL);
 522         robottype_publisher_bumper_create(&orte, NULL, NULL);
 523
 524         /* creating subscribers */
 525         robottype_subscriber_pwr_ctrl_create(&orte,
 526                                 rcv_pwr_ctrl_cb, &orte);
 527         robottype_subscriber_pwr_alert_create(&orte,
 528                                 rcv_pwr_alert_cb, &orte);
 529         robottype_subscriber_motion_speed_create(&orte,
 530                                 rcv_motion_speed_cb, &orte);
 531         robottype_subscriber_can_msg_create(&orte,
 532                                 rcv_can_msg_cb, &orte);
 533         robottype_subscriber_servos_create(&orte,
 534                                 rcv_servos_cb, &orte);
 535         robottype_subscriber_laser_cmd_create(&orte,
 536                                 rcv_laser_cmd_cb, &orte);
 537
 538         robottype_subscriber_drives_create(&orte,
 539                                 rcv_drives_cb, &orte);
 540         /* main loop */
 541         for(;;) {
 542                 FD_ZERO(&read_fd_set);
 543                 FD_SET(sock, &read_fd_set);
 544                 timeout.tv_sec = 0;
 545                 timeout.tv_usec = 10000;
 546
 547                 ret = select(FD_SETSIZE, &read_fd_set, NULL, NULL, &timeout);
 548                 if (ret < 0) {
 549                         perror("cand: select() error");
 550                         goto err;
 551                 }
 552
 553                 /* timeout expired, nothing to be done */
 554                 if (ret == 0)
 555                         continue;
 556
 557                 if (!FD_ISSET(sock, &read_fd_set))
 558                         continue;
 559
 560                 /* read data from SocketCAN */
 561                 size = read(sock, &frame, sizeof(struct can_frame));
 562
 563                 /* parse data */
 564                 cand_parse_frame(&orte, frame);
 565         }
 566
 567 err:
 568         close(sock);
 569         return 1;
 570 out:
 571         return EXIT_SUCCESS;
 572 }