Bool EmApplicationFltk::Startup (int argc, char** argv)
{
#ifdef __QNXNTO__
// speed NTO up so the timing will be about right (for an x86 anyway)
struct _clockperiod cpnew;
struct _clockperiod old;
cpnew.nsec = 5000000;
cpnew.fract = 0;
ClockPeriod(CLOCK_REALTIME, &cpnew, &old, 0);
// printf ("%d %d %d\n", old.nsec, old.fract);
#endif
// Initialize the base system.
if (!EmApplication::Startup (argc, argv))
return false;
// Create our window.
this->PrvCreateWindow (argc, argv);
// Start up the sub-systems.
::MenuInitialize (false);
// Start the clipboard idling.
// !!! Get rid of this special clipboard window. I think that
// we can roll this functionality into fAppWindow.
(void) this->PrvGetClipboardWidget ();
return true;
}
static Boolean
setFrequency (ClockDriver *self, double adj, double tau) {
if (self->config.readOnly){
DBGV("adjFreq2: noAdjust on, returning\n");
return FALSE;
}
self->_tau = tau;
/*
* adjFreq simulation for QNX: correct clock by x ns per tick over clock adjust interval,
* to make it equal adj ns per second. Makes sense only if intervals are regular.
*/
#ifdef __QNXNTO__
struct _clockadjust clockadj;
struct _clockperiod period;
if (ClockPeriod (CLOCK_REALTIME, 0, &period, 0) < 0)
return FALSE;
adj = clampDouble(adj, self->maxFrequency);
/* adjust clock for the duration of 0.9 clock update period in ticks (so we're done before the next) */
clockadj.tick_count = 0.9 * tau * 1E9 / (period.nsec + 0.0);
/* scale adjustment per second to adjustment per single tick */
clockadj.tick_nsec_inc = (adj * tau / clockadj.tick_count) / 0.9;
DBGV("QNX: adj: %.09f, dt: %.09f, ticks per dt: %d, inc per tick %d\n",
adj, tau, clockadj.tick_count, clockadj.tick_nsec_inc);
if (ClockAdjust(CLOCK_REALTIME, &clockadj, NULL) < 0) {
DBGV("QNX: failed to call ClockAdjust: %s\n", strERROR(THIS_COMPONENTerrno));
}
/* regular adjFreq */
#elif defined(HAVE_SYS_TIMEX_H)
DBG2(" adjFreq2: call adjfreq to %.09f us \n", adj / DBG_UNIT);
adjFreq_unix(self, adj);
/* otherwise use adjtime */
#else
struct timeval tv;
adj = clampDouble(adj, self->maxFrequency);
tv.tv_sec = 0;
tv.tv_usec = (adj / 1000);
if((tau > 0) && (tau < 1.0)) {
tv.tv_usec *= tau;
}
adjtime(&tv, NULL);
#endif
self->lastFrequency = adj;
return TRUE;
}
void CreateInterrupt(Interrupt *ret, long periodMicros, long sec)
{
//declare variables
struct sigevent event;
struct itimerspec timer;
struct _clockperiod clkper;
struct sched_param param;
timer_t timer_id;
/* Give this thread root permissions to access the hardware */
ThreadCtl( _NTO_TCTL_IO, NULL );
int chid = ChannelCreate( 0 ); //create event channel
/* Set our priority to the maximum, so we won’t get disrupted by anything other than interrupts. */
param.sched_priority = sched_get_priority_max( SCHED_RR );
//set the clock from 10ns to 1microsecond ticks
clkper.nsec = 100000;
clkper.fract = 0;
ClockPeriod ( CLOCK_REALTIME, &clkper, NULL, 0 ); // 1ms
//Set up the pulse width modulation event
event.sigev_notify = SIGEV_PULSE; // most basic message we can send -- just a pulse number
event.sigev_coid = ConnectAttach ( ND_LOCAL_NODE, 0, chid, 0, 0 ); // Get ID that allows me to communicate on the channel
assert ( event.sigev_coid != -1 ); // stop with error if cannot attach to channel
event.sigev_priority = getprio(0);
event.sigev_code = 1023; // arbitrary number assigned to this pulse
event.sigev_value.sival_ptr = (void*)0; // ?? TBD
// Now create the timer and get back the timer_id value for the timer we created.
if ( timer_create( CLOCK_REALTIME, &event, &timer_id ) == -1)
{
perror ( "can’t create timer" );
exit( EXIT_FAILURE );
}
/* Change the timer request to alter the behavior. */
timer.it_value.tv_sec = sec;
timer.it_value.tv_nsec = periodMicros * 1000;
timer.it_interval.tv_sec = sec;
timer.it_interval.tv_nsec = periodMicros * 1000;
//ret = (Interrupt){timer_id, timer, chid, periodMicros};
ret->timer_id = timer_id;
ret->timer = timer;
ret->chid = chid;
ret->period = periodMicros;
}
int
adjtime (struct timeval *delta, struct timeval *olddelta)
{
double delta_nsec;
double delta_nsec_old;
struct _clockadjust adj;
struct _clockadjust oldadj;
/*
* How many nanoseconds are we adjusting?
*/
if (delta != NULL)
delta_nsec = 1e9 * (long)delta->tv_sec + 1e3 * delta->tv_usec;
else
delta_nsec = 0;
/*
* Build the adjust structure and call ClockAdjust()
*/
if (delta_nsec != 0)
{
struct _clockperiod period;
long count;
long increment;
long increment_limit;
int isneg = 0;
/*
* Convert to absolute value for future processing
*/
if (delta_nsec < 0)
{
isneg = 1;
delta_nsec = -delta_nsec;
}
/*
* Get the current clock period (nanoseconds)
*/
if (ClockPeriod (CLOCK_REALTIME, 0, &period, 0) < 0)
return -1;
/*
* Compute count and nanoseconds increment
*/
count = 1e9 * ADJUST_PERIOD / period.nsec;
increment = delta_nsec / count + .5;
/* Reduce relative error */
if (count > increment + 1)
{
increment = 1 + (long)((delta_nsec - 1) / count);
count = delta_nsec / increment + .5;
}
/*
* Limit the adjust increment to appropriate value
*/
increment_limit = CORR_SLEW_LIMIT * period.nsec;
if (increment > increment_limit)
{
increment = increment_limit;
count = delta_nsec / increment + .5;
/* Reduce relative error */
if (increment > count + 1)
{
count = 1 + (long)((delta_nsec - 1) / increment);
increment = delta_nsec / count + .5;
}
}
adj.tick_nsec_inc = isneg ? -increment : increment;
adj.tick_count = count;
}
else
{
adj.tick_nsec_inc = 0;
adj.tick_count = 0;
}
if (ClockAdjust (CLOCK_REALTIME, &adj, &oldadj) < 0)
return -1;
/*
* Build olddelta
*/
delta_nsec_old = (double)oldadj.tick_count * oldadj.tick_nsec_inc;
if (olddelta != NULL)
{
if (delta_nsec_old != 0)
{
/* Reduce rounding error */
delta_nsec_old += (delta_nsec_old < 0) ? -500 : 500;
olddelta->tv_sec = delta_nsec_old / 1e9;
olddelta->tv_usec = (long)(delta_nsec_old - 1e9
* (long)olddelta->tv_sec) / 1000;
}
else
{
olddelta->tv_sec = 0;
olddelta->tv_usec = 0;
}
}
return 0;
}
// ****************************************************************
// WriteToFile()
//
int DataLogging::WriteToFile(void)
{
int ReturnValue = 0;
unsigned int StartIndex = 0
, StopIndex = 0
, Counter = 0
, i = 0
, ElementsPerLine = 0;
if (this->CurrentObjectState != DataLogging::PrepareLoggingCalled)
{
return(EPERM);
}
else
{
this->CurrentObjectState = DataLogging::WriteToFileCalled;
}
//REMOVE
//------------------------
#ifdef _NTO_
struct _clockperiod ClockResolution;
ClockResolution.nsec = 1000000; //ns
ClockResolution.fract = 0;
ClockPeriod(CLOCK_REALTIME, &ClockResolution, NULL, 0);
#endif
//------------------------
if (this->OutputCounter > this->MaximumNumberOfEntries)
{
StartIndex = (this->OutputCounter + 1) % this->MaximumNumberOfEntries;
StopIndex = StartIndex + this->MaximumNumberOfEntries - 1;
}
else
{
StartIndex = 0;
StopIndex = this->OutputCounter - 1;
}
switch (this->CurrentControlScheme)
{
case FastResearchInterface::JOINT_POSITION_CONTROL:
ElementsPerLine = 2 + 4 * LBR_MNJ + 6;
break;
case FastResearchInterface::CART_IMPEDANCE_CONTROL:
ElementsPerLine = 2 + 3 * FRI_CART_VEC + 6;
break;
case FastResearchInterface::JOINT_IMPEDANCE_CONTROL:
ElementsPerLine = 2 + 8 * LBR_MNJ + 6;
break;
default:
return(EINVAL);
}
while (StopIndex >= StartIndex)
{
Counter++;
fprintf(this->OutputFileHandler, "%d", Counter);
for (i = 0; i < ElementsPerLine; i++)
{
fprintf(this->OutputFileHandler, " %12.6f", this->LoggingMemory[i][StartIndex]);
}
fprintf(this->OutputFileHandler, "\n");
StartIndex++;
}
fflush(this->OutputFileHandler);
ReturnValue = fclose(this->OutputFileHandler);
if (ReturnValue == 0)
{
return(EOK);
}
else
{
return(ReturnValue);
}
}
// ****************************************************************
// PrepareLogging()
//
int DataLogging::PrepareLogging( const unsigned int &ControlScheme
, const char *FileIdentifier)
{
char TimeString[TIME_STRING_LENGTH];
unsigned int i = 0;
time_t CurrentDayTime;
memset( TimeString
, 0x0
, TIME_STRING_LENGTH * sizeof(char));
this->CurrentControlScheme = ControlScheme;
if (this->CurrentObjectState == DataLogging::PrepareLoggingCalled)
{
this->WriteToFile();
}
GetSystemTimeInSeconds(true);
//REMOVE
//------------------------
#ifdef _NTO_
struct _clockperiod ClockResolution;
ClockResolution.nsec = 10000; //ns
ClockResolution.fract = 0;
ClockPeriod(CLOCK_REALTIME, &ClockResolution, NULL, 0);
//------------------------
#endif
memset( this->CompleteOutputFileString
, 0x0
, OUTPUT_FILE_STRING_LENGTH * sizeof(char));
for (i = 0; i < NUMBER_OF_ELEMENTS_PER_ENTRY; i++)
{
memset( this->LoggingMemory[i]
, 0x0
, this->MaximumNumberOfEntries * sizeof(float) );
}
CurrentDayTime = time(NULL);
strftime(TimeString, TIME_STRING_LENGTH, "%y%m%d-%H%M%S", localtime(&CurrentDayTime));
if (FileIdentifier == NULL)
{
sprintf(this->CompleteOutputFileString, "%s%s-%s-%s", this->OutputPath, TimeString, this->MachineName, this->OutputFileName);
}
else
{
sprintf(this->CompleteOutputFileString, "%s%s-%s-%s-%s", this->OutputPath, TimeString, FileIdentifier, this->MachineName, this->OutputFileName);
}
if ( (this->OutputFileHandler = fopen(this->CompleteOutputFileString, "w") ) == NULL)
{
return(EBADF);
}
else
{
fprintf(this->OutputFileHandler, "Logging file of the KUKA Fast Research Interface: %s\n", this->CompleteOutputFileString);
fprintf(this->OutputFileHandler, "This file contains all important control values and importable to Matlab and MS Excel.\n");
fprintf(this->OutputFileHandler, "%s\n", ctime( &CurrentDayTime));
fprintf(this->OutputFileHandler, "Robot name: %s\n", this->MachineName);
switch (this->CurrentControlScheme)
{
case FastResearchInterface::JOINT_POSITION_CONTROL:
fprintf(this->OutputFileHandler, "Active control scheme: joint position control\n\n");
fprintf(this->OutputFileHandler, "Counter KRCTime LocalTime ActFJ1 ActFJ2 ActFJ3 ActFJ4 ActFJ5 ActFJ6 ActFJ7 UDesJ1 UDesJ2 UDesJ3 UDesJ4 UDesJ5 UDesJ6 UDesJ7 ActJ1 ActJ2 ActJ3 ActJ4 ActJ5 ActJ6 ActJ7 KDesJ1 KDesJ2 KDesJ3 KDesJ4 KDesJ5 KDesJ6 KDesJ7 Fx Fy Fz Tx Ty Tz\n");
break;
case FastResearchInterface::CART_IMPEDANCE_CONTROL:
fprintf(this->OutputFileHandler, "Active control scheme: Cartesian impedance control\n\n");
fprintf(this->OutputFileHandler, "Counter KRCTime LocalTime DesKx DesKy DesKz DesKa DesKb DesKc DesDx DesDy DesDz DesDa DesDb DesDc UDesFx UDesFy UDesFz UDesFa UDesFb UDesFc Fx Fy Fz Tx Ty Tz\n");
break;
case FastResearchInterface::JOINT_IMPEDANCE_CONTROL:
fprintf(this->OutputFileHandler, "Active control scheme: joint impedance control\n\n");
fprintf(this->OutputFileHandler, "Counter KRCTime LocalTime ActFJ1 ActFJ2 ActFJ3 ActFJ4 ActFJ5 ActFJ6 ActFJ7 UDesPJ1 UDesPJ2 UDesPJ3 UDesPJ4 UDesPJ5 UDesPJ6 UDesPJ7 ActPJ1 ActPJ2 ActPJ3 ActPJ4 ActPJ5 ActPJ6 ActPJ7 KDesPJ1 KDesPJ2 KDesPJ3 KDesPJ4 KDesPJ5 KDesPJ6 KDesPJ7 DesKJ1 DesKJ2 DesKJ3 DesKJ4 DesKJ5 DesKJ6 DesKJ7 DesDJ1 DesDJ2 DesDJ3 DesDJ4 DesDJ5 DesDJ6 DesDJ7 UDesFJ1 UDesFJ2 UDesFJ3 UDesFJ4 UDesFJ5 UDesFJ6 UDesFJ7 KOffPJ1 KOffPJ2 KOffPJ3 KOffPJ4 KOffPJ5 KOffPJ6 KOffPJ7 Fx Fy Fz Tx Ty Tz\n");
break;
default:
return(EINVAL);
}
}
fflush(this->OutputFileHandler);
this->CurrentObjectState = DataLogging::PrepareLoggingCalled;
this->OutputCounter = 0;
return(EOK);
}
