SyncInterrupt.cpp

 
#ifdef __INTIME_CPP20 
 
#include <stdio.h>
#include "rt.h"
#include "rsi.h"
#include "HelperFunctions.h"
 
using namespace RSI::RapidCode;
 
//configurable
constexpr int32_t SYNC_PERIOD = 1;            // interrupt every MotionController sample
constexpr int32_t AXIS_COUNT = 2;             // how many axes will we process each sample
constexpr double MICROSECONDS_PER_SECOND = 1000000.0;
 
constexpr int32_t LOW_PRIORITY = 200;
constexpr int32_t SLOW_LOOP_MILLISECONDS = 50;
 
 
// RapidCode objects
MotionController* controller;
Axis* axes[AXIS_COUNT];
 
// shared variables
double encoderPositions[AXIS_COUNT];
int32_t currentPerformanceCounter = 0;
int32_t previousPerformanceCounter = 0;
int32_t deltaPerformanceCounter = 0;
int32_t iterations = 0;
uint32_t cpuFrequency = 0;
double deltaMicroseconds = 0.0;
double minMicroseconds = 1000000.0;
double maxMicroseconds = 0.0;
int32_t rmpSampleCounter;
int32_t syncInterruptSampleCounter;
int32_t lastSyncInterruptSampleCounter;
bool readyToCleanup = false;
 
RTHANDLE WaitForSystemNotificationsThread = NULL;
RTHANDLE WaitKeyPressAndPrintThread = NULL;
 
void SyncInterruptMainLoop()
{
  // configure a Sync interrupt for every sample
  controller->SyncInterruptPeriodSet(SYNC_PERIOD);
 
  // enable controller interrupts
  controller->SyncInterruptEnableSet(true);
 
  // get initial value
  lastSyncInterruptSampleCounter = controller->SyncInterruptWait();
 
  // the slow thread waiting for a key press will set readyToCleanup = true 
  while (!readyToCleanup)
  {
    syncInterruptSampleCounter = controller->SyncInterruptWait();
    if (syncInterruptSampleCounter != (lastSyncInterruptSampleCounter + 1))
    {
      printf("Sync Interrupt not working as expected. syncCounter %ld, lastSyncCounter %ld\n", syncInterruptSampleCounter, lastSyncInterruptSampleCounter);
      readyToCleanup = true;
    }
 
    // see how int32_t it's been since last interrupt
    currentPerformanceCounter = controller->OS->PerformanceTimerCountGet();
    deltaPerformanceCounter = currentPerformanceCounter - previousPerformanceCounter;
    previousPerformanceCounter = currentPerformanceCounter;
    deltaMicroseconds = (double)(deltaPerformanceCounter * (double)(1 / (double)cpuFrequency)) * MICROSECONDS_PER_SECOND;
    if (iterations > 1) // ignore first time through
    {
      if (deltaMicroseconds > maxMicroseconds)
      {
        maxMicroseconds = deltaMicroseconds;
      }
      if (deltaMicroseconds < minMicroseconds)
      {
        minMicroseconds = deltaMicroseconds;
      }
    }
 
    // Get Encoder Positions
    for (int32_t i = 0; i < AXIS_COUNT; i++)
    {
      encoderPositions[i] = axes[i]->EncoderPositionGet(RSIMotorFeedback::RSIMotorFeedbackPRIMARY);
    }
 
    // do calculations here
 
 
    // set output here 
 
 
    // update the last counter value
    lastSyncInterruptSampleCounter = syncInterruptSampleCounter;
 
    // used for printing info
    iterations++;
 
  }
 
  // turn off Sync Interrupt
  controller->SyncInterruptEnableSet(false);
}
 
 
void WaitForWaitKeyPressAndPrint(void)
{
  // wait for someone to press a key 
  while (controller->OS->KeyGet((int32_t)RSIWait::RSIWaitPOLL) < 0 && readyToCleanup == false)
  {
    RtSleep(SLOW_LOOP_MILLISECONDS);
    printf("SyncInterrupts processed %ld.\t Time since last interrupt: %0.1lfus  Min: %0.0lfus Max: %0.0lfus\r", iterations, deltaMicroseconds, minMicroseconds, maxMicroseconds);
 
  }
  printf("Key pressed.\n");
  readyToCleanup = true;
}
 
 
void WaitForSystemNotifications(void)
{
  EVENTINFO eiEventInfo;
  // wait for notification
  while (RtNotifyEvent(RT_SYSTEM_NOTIFICATIONS | RT_EXIT_NOTIFICATIONS,
    WAIT_FOREVER,
    &eiEventInfo))
  {
    switch (eiEventInfo.dwNotifyType)
    {
    case TERMINATE:
    case NT_HOST_SHUTDOWN_PENDING:
    case KERNEL_STOPPING:
    case KERNEL_SHUTDOWN_PENDING:
    case RT_CLIENT_DOWN:
    case RT_CLIENT_UP:
    case NT_HOST_DOWN:
    case NT_HOST_UP:
    case NT_BLUESCREEN:
      readyToCleanup = true;
      break;
    }
  }
}
 
void Cleanup(void)
{
  if (WaitForSystemNotificationsThread != NULL)
  {
    DeleteRtThread(WaitForSystemNotificationsThread);
  }
 
  if (WaitKeyPressAndPrintThread != NULL)
  {
    DeleteRtThread(WaitKeyPressAndPrintThread);
  }
 
  if (controller != nullptr)
  {
    // Delete the controller to clean up all RapidCodeRT objects
    controller->Delete();
  }
}
 
int32_t main(int32_t argc, char* argv[])
{
  try
  {
    printf("Hello, RapidCodeRT!\n");
 
    // create and Initialize MotionController class. 
    controller = MotionController::CreateFromSoftware();
    HelperFunctions::CheckErrors(controller);
 
    printf("Serial Number: %d \n", controller->SerialNumberGet());
 
    int32_t axisCount = controller->AxisCountGet();
    if (axisCount < AXIS_COUNT)
    {
      printf("You don't have enough Axis objects to run this sample. You have %d but need %d.\n", axisCount, AXIS_COUNT);
      exit(1);
    }
 
    // get Axis objects from MotionController
    for (int i = 0; i < AXIS_COUNT; i++)
    {
      axes[i] = controller->AxisGet(i);
      HelperFunctions::CheckErrors(axes[i]);
    }
 
    // disable the service thread if using the controller Sync interrupt0
    controller->ServiceThreadEnableSet(false);
 
    // Get CPU frequency from Operating System performance counter
    cpuFrequency = controller->OS->PerformanceTimerFrequencyGet();
    printf("CPU Frequency is: %u Hz\n", cpuFrequency);
 
    // start two slow polling threads
    WaitForSystemNotificationsThread = CreateRtThread(LOW_PRIORITY, (LPPROC)WaitForSystemNotifications, 1024, NULL);
    WaitKeyPressAndPrintThread = CreateRtThread(LOW_PRIORITY, (LPPROC)WaitForWaitKeyPressAndPrint, 1024, NULL);
 
    // run the high priority loop
    SyncInterruptMainLoop();
 
    // after the loop is finished, cleanup MotionController and threads.
    Cleanup();
  }
  catch (std::exception const& e)
  {
    printf("\n%s\n", e.what());
  }
 
 
  return 0;
}
 
#endif // __INTIME_CPP20