Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- int IMU_thread()
- {
- printf("Welcome to the serial test app!\n\n");
- Serial* SP = new Serial("\\\\.\\COM6"); // adjust as needed
- if (SP->IsConnected())
- printf("We're connected");
- else
- printf("IMU CONNECTION FAILED!!");
- char incomingData[25] = ""; // don't forget to pre-allocate memory
- //printf("%s\n",incomingData);
- int dataLength = 500;
- int readResult = 0;
- int toMove = 0;
- int numtimes = 0;
- clock_t t;
- t = clock();
- while(SP->IsConnected())
- {
- memset(incomingData,0,25);
- readResult = 0;
- char coord[10];
- int coordopen = 0;
- int coord_len = 0;
- while (readResult <= 0 || incomingData[readResult-1] != '\n')
- {
- // read a raw byte
- if (SP->ReadData(incomingData + readResult,1) == 1)
- readResult++;
- else
- continue;
- // if not special demarcating character, keep adding to same coord
- if (coordopen > 0 && incomingData[readResult-1] >= 44)
- {
- coord[coord_len] = incomingData[readResult-1];
- coord_len++;
- }
- // tab
- else if (incomingData[readResult-1] == 9)
- {
- // store coord if have been writing one
- if (coordopen > 0)
- {
- coord[coord_len] = '\n';
- coords[coordopen-1] = atof(coord);
- }
- // prepare for new coord
- coordopen++;
- coord_len = 0;
- memset(coord,0,10);
- }
- // record last coord
- else if (incomingData[readResult-1] == '\n' && coordopen > 0)
- {
- mtx.lock();
- coords[coordopen-1] = atof(coord);
- mtx.unlock();
- }
- }
- // printf("Bytes read: (-1 means no data available) %i\n",readResult);
- }
- return 0;
- }
Advertisement
Add Comment
Please, Sign In to add comment