int main()
{
SerialPort sp;
cout << sp.Connect("COM3") << endl;
cout << sp.Setup() << endl;
// Create a message
RobotCommand msg;
UnitStatus stat;
SetStatus(stat, 0, 0, 0, 1, 1);
// Write it to device
while (1)
{
char c = getch();
switch (c)
{
case 'w':
SetMessage(msg, CMD_FORWARD, 50); // Forward with 50/255 speed
break;
case 'a':
SetMessage(msg, CMD_LEFT, 10); // Left with 10/255 of max
break;
case 's':
SetMessage(msg, CMD_BACKWARD, 20); // Backward with 20/255
break;
case 'd':
SetMessage(msg, CMD_RIGHT, 5);
break;
case 'q':
SetMessage(msg, CMD_STOP, 0); // Stop with no braking
case 'e':
SetMessage(msg, CMD_STOP, 255); // Stop with full braking
break;
default:
cout << "## Command not recognized ##" << endl << endl;
continue;
}
cout << "Sending message: " << endl << "------------------------" << endl << msg << endl << endl;
sp.WriteMessage(msg);
cout << "Fetching robot status:" << endl << "------------------------" << endl;
SetMessage(msg, CMD_GET_CURRENT_STATE);
sp.WriteMessage(msg);
int res = sp.ReadMessage(stat);
if (res)
cout << "Failed to read message: " << res << endl;
else
cout << stat << endl;
}
sp.Close();
cout << endl << "End" << endl;
return 0;
}
void ControlC( int sigNum )
{
fprintf( stderr, "Shutting down...\n" );
// Closing the serial port will cause the ReadSerialThread to unblock and exit
gSerialPort.Close();
if ( tcsetattr( fileno( stdin ), TCSANOW, &gTio_org ) == -1 )
{
LogError( "Unable to restore terminal settings\n" );
}
if ( gLogFs2 != NULL )
{
fclose( gLogFs2 );
}
// Now do whatever the default handling for the signal would be
signal( sigNum, SIG_DFL );
raise( sigNum );
} // ControlC
int main( int argc, char **argv )
{
int sig;
sigset_t termSig;
pthread_t readSerialThreadId;
pthread_t readStdinThreadId;
int rc;
int opt;
const char *baudStr = NULL;
#if defined( linux )
const char *portStr = "ttyS0";
#else
const char *portStr = "com1";
#endif
#if USE_I2C
PKT_TextChar = PacketTextChar;
PKT_SendChar = PacketSendChar;
PKT_PacketReceived = PacketReceived;
#endif
// signal( SIGINT, ControlC );
// signal( SIGTERM, ControlC );
LogInit( stdout );
while (( opt = getopt_long( argc, argv, "b:dhmp:rsv", gLongOption, NULL )) > 0 )
{
switch ( opt )
{
case 'b':
{
baudStr = optarg;
break;
}
case 'd':
{
gDebug = true;
break;
}
case 'g':
{
gDongle = true;
break;
}
case 'm':
{
gMegaLoad = true;
break;
}
case 'p':
{
portStr = optarg;
break;
}
case 'r':
{
gUseRtsToReset = true;
break;
}
case 's':
{
gStk500 = true;
break;
}
case 'v':
{
gVerbose = true;
break;
}
#if USE_I2C
case OPT_DEBUG_DONGLE:
{
gSerialDongle.m_debugDongle = true;
break;
}
case OPT_DEBUG_DONGLE_DATA:
{
gSerialDongle.m_debugDongleData = true;
break;
}
#endif
case '?':
case 'h':
{
Usage();
return 1;
}
}
}
if ( optind < argc )
{
if (( optind + 1 ) != argc )
{
fprintf( stderr, "Only one download file supported\n" );
return 1;
}
gDownloadFileName = argv[ optind ];
}
// Open the file to download
if ( gDownloadFileName != NULL )
{
// If we are asked to download a file, then read the entire file
// into memory.
if (( gDownloadInfo = ReadFile( gDownloadFileName )) == NULL )
{
return 1;
}
}
if ( !gSerialPort.Open( portStr, baudStr ))
{
return 1;
}
if ( gUseRtsToReset )
{
gSerialPort.StrobeRTS( 10 );
}
// Put stdin in raw mode
setbuf( stdin, NULL );
setbuf( stdout, NULL );
#if defined( unix )
sigemptyset( &termSig );
sigaddset( &termSig, SIGINT );
sigaddset( &termSig, SIGTERM );
pthread_sigmask( SIG_BLOCK, &termSig, NULL );
struct termios tio_new;
if ( tcgetattr( fileno( stdin ), &gTio_org ) < 0 )
{
LogError( "Unable to retrieve terminal settings\n" );
return 1;
}
tio_new = gTio_org;
tio_new.c_lflag &= ~( ICANON | ECHO );
tio_new.c_cc[VMIN] = 1;
tio_new.c_cc[VTIME] = 0;
if ( tcsetattr( fileno( stdin ), TCSANOW, &tio_new ) < 0 )
{
LogError( "Unable to update terminal settings\n" );
return 1;
}
#endif
const char *bootLoaderType = "*** Unknown ***";
if ( gDongle )
{
bootLoaderType = "Serial Dongle";
}
else
if ( gMegaLoad )
{
bootLoaderType = "MegaLoad v2.3";
}
else
if ( gStk500 )
{
bootLoaderType = "STK500";
}
gLogFs2 = fopen( "BootHost.log", "wb" );
Log( "BootHost - BootLoader: %s\n", bootLoaderType );
// Kick off the serial port reader thread.
rc = pthread_create( &readSerialThreadId, NULL, ReadSerialThread, &gSerialPort );
if ( rc != 0 )
{
fprintf( stderr, "Error creating ReadSerialThread: %d\n", rc );
return 1;
}
// Kick off the stdin reader thread.
rc = pthread_create( &readStdinThreadId, NULL, ReadStdinThread, NULL );
if ( rc != 0 )
{
fprintf( stderr, "Error creating ReadSerialThread: %d\n", rc );
return 1;
}
#if defined( unix )
// Wait for a termmination signal
if (( rc = sigwait( &termSig, &sig )) != 0 )
{
fprintf( stderr, "sigwait failed\n" );
}
else
{
fprintf( stderr, "Exiting...\n" );
}
pthread_cancel( readSerialThreadId );
pthread_cancel( readStdinThreadId );
// Restore stdin back to the way it was when we started
if ( tcsetattr( fileno( stdin ), TCSANOW, &gTio_org ) == -1 )
{
LogError( "Unable to restore terminal settings\n" );
}
#endif
#if defined( __CYGWIN__ )
// Under Windows closing the serial port and stdin will cause the reads
// to unblock. Under linux, this isn't required, but it doesn't hurt
// either.
gSerialPort.Close();
fclose( stdin );
#endif
// Unblock the termination signals so the user can kill us if we hang up
// waiting for the reader threads to exit.
#if defined( unix )
pthread_sigmask( SIG_UNBLOCK, &termSig, NULL );
#endif
pthread_join( readSerialThreadId, NULL );
pthread_join( readStdinThreadId, NULL );
#if !defined( __CYGWIN__ )
gSerialPort.Close();
fclose( stdin );
#endif
if ( gVerbose )
{
printf( "Done\n" );
}
return 0;
} // main