static int RCMainLine( const char *opts, int argc, char **argv )
/**************************************************************/
{
char *cmdbuf = NULL;
const char *str;
char infile[_MAX_PATH + 2]; // +2 for quotes
char outfile[_MAX_PATH + 6]; // +6 for -fo="" or -fe=""
bool pass1;
int i;
int rc;
curBufPos = formatBuffer;
RcMemInit();
InitGlobs();
rc = setjmp( jmpbuf_RCFatalError );
if( rc == 0 ) {
InitRcMsgs();
if( opts != NULL ) {
str = opts;
argc = ParseEnvVar( str, NULL, NULL );
argv = RcMemMalloc( ( argc + 4 ) * sizeof( char * ) );
cmdbuf = RcMemMalloc( strlen( str ) + argc + 1 );
ParseEnvVar( str, argv, cmdbuf );
pass1 = false;
for( i = 0; i < argc; i++ ) {
if( argv[i] != NULL && !stricmp( argv[i], "-r" ) ) {
pass1 = true;
break;
}
}
if( initInfo != NULL && initInfo->ver > 1 && initInfo->cmd_line_has_files ) {
if( !ideCb->GetInfo( cbHandle, IDE_GET_SOURCE_FILE, 0, (IDEGetInfoLParam)( infile + 1 ) ) ) {
infile[0] = '\"';
strcat( infile, "\"" );
argv[argc++] = infile;
}
if( !ideCb->GetInfo( cbHandle, IDE_GET_TARGET_FILE, 0, (IDEGetInfoLParam)( outfile + 5 ) ) ) {
if( pass1 ) {
strcpy( outfile, "-fo=\"" );
} else {
strcpy( outfile, "-fe=\"" );
}
strcat( outfile, "\"" );
argv[argc++] = outfile;
}
}
argv[argc] = NULL; // last element of the array must be NULL
}
if( !ScanParams( argc, argv ) ) {
rc = 1;
}
if( !CmdLineParms.Quiet ) {
RcIoPrintBanner();
}
if( CmdLineParms.PrintHelp ) {
RcIoPrintHelp();
}
if( rc == 0 ) {
rc = RCSpawn( RCmain );
}
if( opts != NULL ) {
RcMemFree( argv );
RcMemFree( cmdbuf );
}
FiniRcMsgs();
}
FiniGlobs();
flushPrintf();
RcMemShutdown();
return( rc );
}
//=========================================================================
// fri_runDemoCommandMode
// Run a demo using the "command" mode (sinewave motion of the robot)
//=========================================================================
void fri_runDemoCommandMode(int listenningPort) {
// Initializations
fri_checkSetup();
friRemote friInst(listenningPort);
FRI_QUALITY lastQuality = FRI_QUALITY_UNACCEPTABLE;
double timeCounter = 0;
// Send the command
SET_COMMAND_FLAG(COMMAND_DEMO_COMMAND_MODE);
friInst.doDataExchange();
// Wait for the controller to acknowledge the command (busy flag set)
while (!GET_BUSY_FLAG())
friInst.doDataExchange();
// Reset the command
SET_COMMAND_FLAG(COMMAND_NONE);
friInst.doDataExchange();
// Wait to be in 'command' mode
while (1) {
float jointValues[LBR_MNJ];
for (int i = 0; i < LBR_MNJ; i++)
jointValues[i] = friInst.getMsrCmdJntPosition()[i]; // Get current joint values
friInst.doPositionControl(jointValues); // Exchange packets
if (friInst.getQuality() != lastQuality) { // Connection quality just changed
printf("Connection quality changed (%d -> %d)\n", lastQuality, friInst.getQuality());
lastQuality = friInst.getQuality();
}
// Check conditions for breaking the loop
if (friInst.getState() == FRI_STATE_CMD && friInst.isPowerOn()) // We are in 'command' mode
break; // Go on
if (!GET_BUSY_FLAG()) // Something's wrong on the controller side: it's not busy anymore
return; // Stop
}
// We are in 'command' mode
timeCounter = 0.0;
while (1) {
printf("%f\n", timeCounter);
float jointValues[LBR_MNJ];
for (int i = 0; i < LBR_MNJ; i++)
jointValues[i] = friInst.getMsrCmdJntPosition()[i]; // Get current joint values
// Prepare new joint values
for (int i = 0; i < LBR_MNJ; i++)
jointValues[i] += (float) sin(timeCounter * PI * 0.020 * 20 * 0.5) * (float) DEG2RAD(10.0); // Sine wave motion
timeCounter += friInst.getSampleTime(); // Increment time counter
if (timeCounter > 10.0) // Enough time elapsed
SET_COMMAND_FLAG(COMMAND_STOP); // Clean stop
friInst.doPositionControl(jointValues); // Exchange packets
// Display debug information
if (friInst.getSequenceCount() % 40 == 0) { // Every 40th packet
printf("\tTime: %.2f\n", timeCounter); flushPrintf();
}
// Check conditions for breaking the loop
if (!GET_BUSY_FLAG()) // Something's wrong on the controller side: it's not busy anymore
break;
if (friInst.getState() != FRI_STATE_CMD) // We are not in command mode anymore
break;
}
}
