Guest User

IMU

a guest
Jul 16th, 2014
448
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 1.56 KB | None | 0 0
  1. int IMU_thread()
  2. {
  3.     printf("Welcome to the serial test app!\n\n");
  4.     Serial* SP = new Serial("\\\\.\\COM6");    // adjust as needed
  5.     if (SP->IsConnected())
  6.         printf("We're connected");
  7.     else
  8.         printf("IMU CONNECTION FAILED!!");
  9.     char incomingData[25] = "";         // don't forget to pre-allocate memory
  10.     //printf("%s\n",incomingData);
  11.     int dataLength = 500;
  12.     int readResult = 0;
  13.     int toMove = 0;
  14.     int numtimes = 0;
  15.     clock_t t;
  16.     t = clock();
  17.     while(SP->IsConnected())
  18.     {
  19.         memset(incomingData,0,25);
  20.         readResult = 0;
  21.         char coord[10];
  22.         int coordopen = 0;
  23.         int coord_len = 0;
  24.         while (readResult <= 0 || incomingData[readResult-1] != '\n')
  25.         {
  26.             // read a raw byte
  27.             if (SP->ReadData(incomingData + readResult,1) == 1)
  28.                 readResult++;
  29.             else
  30.                 continue;
  31.  
  32.             // if not special demarcating character, keep adding to same coord
  33.             if (coordopen > 0 && incomingData[readResult-1] >= 44)
  34.             {
  35.                 coord[coord_len] = incomingData[readResult-1];
  36.                 coord_len++;
  37.             }
  38.  
  39.             // tab
  40.             else if (incomingData[readResult-1] == 9)
  41.             {
  42.                 // store coord if have been writing one
  43.                 if (coordopen > 0)
  44.                 {
  45.                     coord[coord_len] = '\n';
  46.                     coords[coordopen-1] = atof(coord);
  47.                 }
  48.  
  49.                 // prepare for new coord
  50.                 coordopen++;
  51.                 coord_len = 0;
  52.                 memset(coord,0,10);
  53.             }
  54.             // record last coord
  55.             else if (incomingData[readResult-1] == '\n' && coordopen > 0)
  56.             {
  57.                 mtx.lock();
  58.                 coords[coordopen-1] = atof(coord);
  59.                 mtx.unlock();
  60.             }
  61.         }
  62.     //  printf("Bytes read: (-1 means no data available) %i\n",readResult);
  63.     }
  64.     return 0;
  65.  
  66. }
Advertisement
Add Comment
Please, Sign In to add comment