src/cand/cand.cc

   1 #ifdef HAVE_CONFIG_H
   2 #include <config.h>
   3 #endif
   4
   5 #include <iostream>
   6 #include <cstdlib>
   7 #include <sys/socket.h>
   8 #include <sys/ioctl.h>
   9 #include <fcntl.h>
  10
  11 #include "af_can.h"
  12 #include "lib.h"
  13
  14 #include <can_ids.h>
  15 #include "cand.h"
  16 #include <ortelib.h>
  17 #include <sharp.h>
  18
  19 using namespace std;
  20
  21 short int convertShortInt(short int x)
  22 {
  23         unsigned char lsb;
  24         unsigned char msb;
  25
  26         lsb = x>>8;
  27         msb = x & (0xff);
  28
  29         return (msb<<8)|lsb;
  30 }
  31
  32 int init_can()
  33 {
  34         if ((sock_motor = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
  35                     perror("create motor socket");
  36                     return 1;
  37         }
  38
  39         addr_motor.can_family = AF_CAN;
  40
  41         strcpy(ifr_motor.ifr_name, "can0");
  42         if (ioctl(sock_motor, SIOCGIFINDEX, &ifr_motor) < 0)
  43                 perror("SIOCGIFINDEX");
  44         addr_motor.can_ifindex = ifr_motor.ifr_ifindex;
  45
  46         if (!addr_motor.can_ifindex) {
  47                 perror("invalid motor socket interface");
  48                 return 1;
  49         }
  50
  51         if (bind(sock_motor, (struct sockaddr *)&addr_motor, sizeof(addr_motor)) < 0) {
  52                 perror("motor socket bind");
  53                 return 1;
  54         }
  55
  56         return 0;
  57 }
  58
  59 int set_motor_speed(struct orte_data *orte_data)
  60 {
  61         unsigned char data[4];
  62         data[0] = orte_data->motion_speed.left>>8;
  63         data[1] = orte_data->motion_speed.left & 0xff;
  64         data[2] = orte_data->motion_speed.right >> 8;
  65         data[3] = orte_data->motion_speed.right & 0xff;
  66 //      printf("data: ");
  67 //      for (int i=0; i< 4; i++) {
  68 //              printf("%02x ",data[i]);
  69 //      }
  70 //      printf("\n");
  71         can_send(CAN_MOTION_CMD, 4, data);
  72         return 0;
  73 }
  74
  75 int set_serva(struct orte_data *orte_data)
  76 {
  77         unsigned char data[6];
  78         data[5] = orte_data->servos.backDoor;
  79         data[1] = orte_data->servos.frontDoor;
  80         data[2] = orte_data->servos.transporterFront;
  81         data[3] = orte_data->servos.transporterInner;
  82         data[4] = orte_data->servos.innerDoor;
  83         data[0] = orte_data->servos.release;
  84 //      for(int i=0;i<6;i++) printf("data %d = 0x%x\n",i,data[i]);
  85         can_send(CAN_SERVO, 6, data);
  86         return 0;
  87 }
  88
  89 bool moveServo(int servoNr, int value) {
  90
  91 }
  92
  93 int can_send(canid_t id, unsigned char length, unsigned char *data)
  94 {
  95         struct can_frame frame;
  96         int nbytes;
  97
  98         frame.can_id = id;
  99         frame.can_dlc = length;
 100         for(int i=0; i<length; i++) {
 101                 frame.data[i] = data[i];
 102         }
 103         if ((nbytes = write(sock_motor, &frame, sizeof(struct can_frame))) < 0) {
 104 //              perror("write");
 105                 return 1;
 106         }
 107         else if (nbytes < sizeof(struct can_frame)) {
 108 //              printf("write: incomplete CAN frame\n");
 109                 return 1;
 110         }
 111 //      printf("write returned %d\n",nbytes);
 112 //      printf("finnished can send\n");
 113         return 0;
 114 }
 115
 116 int main(int argc, char *argv[])
 117 {
 118         struct orte_data orte;
 119         int ret;
 120         fd_set read_fd_set;
 121         fd_set master;
 122         struct timeval timeval;
 123         bool f_motor_send;
 124         char nbytes;
 125         int las_mi, las_di, las_bcnt;
 126         unsigned short *las_meas;
 127         int i;
 128
 129         int adc1_cnt=0;
 130         int adc2_cnt=0;
 131         int adc3_cnt=0;
 132         int ir_cnt=0;
 133         int las1_cnt=0;
 134         int las2_cnt=0;
 135         int las3_cnt=0;
 136         int las4_cnt=0;
 137         int status_cnt=0;
 138
 139         if(init_can()) printf("init can failed\n");
 140         else printf("init can OK\n");
 141
 142         ortelib_init(NULL, &orte, ORTE_SUB_MOTOR | ORTE_SUB_SERVA,
 143                     ORTE_PUB_IR | ORTE_PUB_FD | ORTE_PUB_DI | ORTE_PUB_SHARPLONG |
 144                     ORTE_PUB_ADCS | ORTE_PUB_SHARPSHORT | ORTE_PUB_POSITION |
 145                     ORTE_PUB_MOTSTAT | ORTE_PUB_LASER);
 146
 147         for(;;) {
 148                 FD_ZERO(&read_fd_set);
 149                 FD_SET(sock_motor, &read_fd_set);
 150                 timeval.tv_sec = 0;
 151                 timeval.tv_usec = 10000;
 152
 153                 ret = select(FD_SETSIZE, &read_fd_set, NULL, NULL, &timeval);
 154                 if (ret == -1) {
 155                         perror("select()");
 156                         return 1;
 157                 }
 158 //              else if(ret==0) perror("no message received in 10000 usec");
 159                 else {
 160                         nbytes = read(sock_motor, &frame, sizeof(struct can_frame));
 161                         switch(frame.can_id) {
 162                                 case CAN_MOTION_ODOMETRY_SIMPLE: {
 163                                         orte.motion_position.left = (frame.data[0]<<24)|(frame.data[1]<<16)|(frame.data[2]<<8)|(frame.data[3]);
 164                                         orte.motion_position.right = (frame.data[4]<<24)|(frame.data[5]<<16)|(frame.data[6]<<8)|(frame.data[7]);
 165                                         ORTEPublicationSend(orte.publicationMotionPos);
 166                                         break;
 167                                 }
 168                                 case CAN_MOTION_STATUS: {
 169                                         orte.motion_status.err_left = (frame.data[0]<<8)|(frame.data[1]);
 170                                         orte.motion_status.err_right = (frame.data[2]<<8)|(frame.data[3]);
 171                                         if(++status_cnt==5) {
 172                                                 ORTEPublicationSend(orte.publicationMotionStatus);
 173                                                 status_cnt = 0;
 174                                         }
 175 //                                      if(ms.err_left || ms.err_right) printf("MOTOR STATUS: left 0x%x, right 0x%x\n",ms.err_left,ms.err_right);
 176                                         break;
 177                                 }
 178                                 case CAN_ADC_1:
 179                                         /* TODO: zkontrolovat, zda hodnoty jsou spravne v mm !! */
 180                                         orte.sharps_oponent.longSharpDist1 = s_ir2mmLong((frame.data[0]<<8)|(frame.data[1]))/1000.0;
 181                                         orte.sharps_oponent.longSharpDist2 = s_ir2mmLong((frame.data[2]<<8)|(frame.data[3]))/1000.0;
 182                                         orte.sharps_oponent.longSharpDist3 = s_ir2mmLong((frame.data[4]<<8)|(frame.data[5]))/1000.0;
 183                                         orte.front_door.state = (frame.data[6]<<8)|frame.data[7];
 184
 185                                         if(++adc1_cnt == 5) {
 186                                                 ORTEPublicationSend(orte.publicationSharp1);
 187                                                 adc1_cnt = 0;
 188                                         }
 189                                         break;
 190                                 case CAN_ADC_2:
 191                                         orte.sharps_waste.short1 = (frame.data[0]<<8)|(frame.data[1]);
 192                                         orte.sharps_waste.short2 = (frame.data[2]<<8)|(frame.data[3]);
 193                                         orte.sharps_waste.short3 = (frame.data[4]<<8)|(frame.data[5]);
 194                                         orte.sharps_waste.short4 = (frame.data[6]<<8)|(frame.data[7]);
 195
 196                                         if(++adc2_cnt == 5) {
 197                                                 ORTEPublicationSend(orte.publicationSharp2);
 198                                                 adc2_cnt = 0;
 199                                         }
 200                                         break;
 201                                 case CAN_ADC_3:
 202                                         orte.adcs.sharpLong1 = (frame.data[0]);
 203                                         orte.adcs.sharpLong2 = (frame.data[1]);
 204                                         orte.adcs.sharpLong3 = (frame.data[2]);
 205                                         orte.adcs.sharpShort1 = (frame.data[3]);
 206                                         orte.adcs.sharpShort2 = (frame.data[4]);
 207                                         orte.adcs.sharpShort3 = (frame.data[5]);
 208                                         orte.adcs.sharpShort4 = (frame.data[6]);
 209                                         orte.adcs.frontDoor = (frame.data[7]);
 210
 211                                         if(++adc3_cnt == 5) {
 212                                                 ORTEPublicationSend(orte.publicationAdcs);
 213                                                 adc3_cnt = 0;
 214                                         }
 215                                         break;
 216                                 case CAN_IR:
 217                                         orte.inner_ir.front = frame.data[1];
 218                                         orte.inner_ir.back = frame.data[0];
 219 //                                      printf("IR: 0x%04x\n",stateIR);
 220                                         ORTEPublicationSend(orte.publicationIR);
 221                                         orte.dig_in.state = frame.data[2];
 222                                         if(++ir_cnt == 5) {
 223                                                 ORTEPublicationSend(orte.publicationDI);
 224                                                 ir_cnt = 0;
 225                                         }
 226                                         break;
 227
 228                                 case CAN_LAS1:
 229                                         printf("CAN: ");
 230                                         for (i=0; i<frame.can_dlc; i++) {
 231                                                 printf("0x%02x ", frame.data[i]);
 232                                         }
 233                                         printf("can_dlc=%d\n", frame.can_dlc);
 234                                         orte.laser.cnt = frame.data[1];
 235                                         las_bcnt = orte.laser.cnt;
 236                                         last_id = frame.data[0];
 237                                         las_di = 4;
 238                                         orte.laser.period = (frame.data[2]<<8)|(frame.data[3]); //perioda
 239                                         printf("CAN ID=0x%02x: cnt=%d period=%d measures: ", frame.can_id, orte.laser.cnt, orte.laser.period);
 240                                         for (las_mi=0; las_mi<(frame.can_dlc-4)/2; las_mi++) {
 241                                                 switch (las_mi) {
 242                                                         case 0: las_meas = &orte.laser.measures0; break;
 243                                                         case 1: las_meas = &orte.laser.measures1; break;
 244                                                         default: break;
 245                                                 }
 246                                                 *las_meas =
 247                                                         (frame.data[las_di++]<<8)|(frame.data[las_di++]);
 248                                                 printf("0x%02x 0x%02x %u ", frame.data[las_di-2], frame.data[las_di-1], *las_meas);
 249                                         }
 250                                         printf("\n");
 251                                         break;
 252                                 case CAN_LAS2:
 253                                 case CAN_LAS3:
 254                                 case CAN_LAS4:
 255                                         printf("CAN: ");
 256                                         for (i=0; i<frame.can_dlc; i++) {
 257                                                 printf("0x%02x ", frame.data[i]);
 258                                         }
 259                                         printf("can_dlc=%d\n", frame.can_dlc);
 260                                         printf("CAN ID=0x%02x: cnt=%d period=%d measures: ", frame.can_id, orte.laser.cnt, orte.laser.period);
 261                                         if (frame.data[0] != (last_id+(frame.can_id-CAN_LAS1)))
 262                                                 break;
 263                                         las_di = 2;
 264                                         while (las_di < 8 && las_bcnt > 0) {
 265                                                 switch (las_mi) {
 266                                                         case 2: las_meas = &orte.laser.measures2; break;
 267                                                         case 3: las_meas = &orte.laser.measures3; break;
 268                                                         case 4: las_meas = &orte.laser.measures4; break;
 269                                                         case 5: las_meas = &orte.laser.measures5; break;
 270                                                         case 6: las_meas = &orte.laser.measures6; break;
 271                                                         case 7: las_meas = &orte.laser.measures7; break;
 272                                                         case 8: las_meas = &orte.laser.measures8; break;
 273                                                         case 9: las_meas = &orte.laser.measures9; break;
 274                                                         default: break;
 275                                                 }
 276                                                 *las_meas =
 277                                                         (frame.data[las_di++]<<8)|(frame.data[las_di++]);
 278                                                 printf("0x%02x 0x%02x %u ", frame.data[las_di-2], frame.data[las_di-1], *las_meas);
 279                                                 las_mi++;
 280                                                 las_bcnt--;
 281                                         }
 282                                         printf("\n");
 283                                         if (las_bcnt == 0) {
 284                                                 printf("ORTEPublicationSend()\n");
 285                                                 ORTEPublicationSend(orte.publicationLaser);
 286                                         }
 287                                         break;
 288                                 default:
 289 //                                      printf("received CAN msg with unknown id: %x\n",frame.can_id);
 290                                         break;
 291                         }
 292
 293                 }
 294         }
 295
 296         close(sock_motor);
 297         return EXIT_SUCCESS;
 298 }