//***********************************************************
//
// Filename:  USRobotTester.cpp
// Author:  Michael D. Wales
//
// Version History:
//  0.0:  Initial test version
//
//***********************************************************

#include<stdio.h>
#include<string.h>
#include<unistd.h>
#include<fcntl.h>
#include<errno.h>
#include<termios.h>

int main (void)
   {
   printf("US Robot Serial Interface Tester\n");
   
   int SerialFD = 0;
   int NumOfChar;
   unsigned char Input;
   termios PortOptions;
   
   SerialFD = open("/dev/ttyS0", O_RDWR | O_NOCTTY | O_NDELAY);
   
   if (SerialFD == -1)
      {
      printf("Serial port didn't work\n");
      }
   
   fcntl( SerialFD, F_SETFL, 0);
   
   while(1)
      {
      NumOfChar = read( SerialFD, &Input, 1);
      if ( Input == 85)
         {
         NumOfChar = read( SerialFD, &Input, 1);
         if (Input == 126)
            {
            NumOfChar = read( SerialFD, &Input, 1);
            printf("Valid code:  Steer=%i", Input);
            NumOfChar = read( SerialFD, &Input, 1);
            printf("\tSpeed=%i\n", Input);
            }
         else
            printf("Invalid:  %i\n", Input);
         }
      else
         printf("Invalid:  %i\n", Input);
      }
   
   return 0;
   }