UpdateBufferPoints.cpp

 
#include <cassert>
#include "rsi.h" // Import our RapidCode Library. 
#include <cmath>
#include "SampleApps.h"
#include "SampleAppsHelper.h" // Import our SampleApp helper functions.
 
using namespace RSI::RapidCode;                     // Import the RapidCode namespace
 
int UpdateBufferPoints::Run()
{
  /* CONSTANTS */
  // *NOTICE* The following constants must be configured before attempting to run with hardware.
 
  // Axis configuration parameters
  const int AXIS_COUNT = 1;                              // number of axes
  const int AXIS_NUMBER = 0;                             // axis number
 
  const double USER_UNITS = 1048576;                        // counts per unit / user units (set as appropiate)
 
  // Motion parameters
  const double TIME_SLICE = 0.001;                          // 0.001s = 1ms
  const int    REVS = 2;                              // number of revolutions
  const int    RPS = 1;                              // revs / sec
  const int    TOTAL_POINTS = (int)(REVS / TIME_SLICE / RPS); // total number of points
  const int    BUFFER_SZ = 100;                            // Number of points to send in a buffer
  const int    EMPTY_CT = 10;                             // Number of points that remains in the beffer before an e-stop
 
  // To run with hardware, set the USE_HARDWARE flag to true AFTER you have configured the parameters above and taken proper safety precautions.
  USE_HARDWARE = false;
 
  /* SAMPLE APP BODY */
 
  // Initizalize the controller from software w/ multiple axes
  MotionController* controller = MotionController::CreateFromSoftware();
  SampleAppsHelper::CheckErrors(controller);
 
  // Setup the controller for the appropriate hardware configuration.
  if (USE_HARDWARE)
  {
    SampleAppsHelper::SetupControllerForHardware(controller);
  }
  else
  {
    SampleAppsHelper::SetupControllerForPhantoms(controller, AXIS_COUNT, { AXIS_NUMBER });
  }
 
  MultiAxis* multiAxis;
 
  try
  {
    // add an additional axis for the multiaxis supervisor
    controller->MotionCountSet(AXIS_COUNT + 1);
 
    // create the multiaxis using the ID of the first free axis (0 indexed)
    multiAxis = controller->MultiAxisGet(AXIS_COUNT);
    SampleAppsHelper::CheckErrors(multiAxis);
 
    // Get the axis and configure it
    Axis* axis = controller->AxisGet(AXIS_NUMBER);                          // Initialize Axis Class. (Use RapidSetup Tool to see what is your axis number)
    SampleAppsHelper::CheckErrors(axis);                      // [Helper Function] Check that the axis has been initialize correctly.
 
    axis->PositionSet(0);                                                   // Make sure motor starts at position 0 everytime.
    axis->UserUnitsSet(USER_UNITS);                                         // Change your user units.
    axis->ErrorLimitActionSet(RSIAction::RSIActionNONE);                    // Set Error Limit Action.
    axis->Abort();                                                          // If there is any motion happening, abort it.
    axis->ClearFaults();                                                    // Clear faults.>
 
    multiAxis->AxisAdd(axis);                                               // Add the axis to the multiaxis supervisor
 
    // populate the positions and times 
    std::vector<double> positions, times;
    for (int i = 0; i < TOTAL_POINTS; i += AXIS_COUNT)
    {
      positions.push_back(i * TIME_SLICE * RPS);
      times.push_back(TIME_SLICE);
    }
 
    // prepare the controller (and drive)
    multiAxis->Abort();
    multiAxis->ClearFaults();
    assert(multiAxis->StateGet() == RSIState::RSIStateIDLE);
    multiAxis->AmpEnableSet(true);
 
    // reset the motion ID to 0
    multiAxis->MovePT(RSIMotionType::RSIMotionTypePT, &positions[0], &times[0], 1, -1, false, true);
    multiAxis->MotionIdSet(0);
 
    // Establish how to keep track of what blocks have been sent
    int curMotionElementID = 0, curMotionID = 0, finalMotionID = 0;
    int numPointsToSend = BUFFER_SZ;
    int endOfLastSent = 0;
    bool exitCondition = false;
 
    // Set up a motion hold gate so we can start buffering blocks
    const int motionHoldGate = 3;
    controller->MotionHoldGateSet(motionHoldGate, true);
    multiAxis->MotionHoldGateSet(motionHoldGate);
 
    for (int i = 0; i < 2; ++i)
    {
      multiAxis->MovePT(RSIMotionType::RSIMotionTypePT, &positions[0] + endOfLastSent * AXIS_COUNT, &times[0] + endOfLastSent, numPointsToSend, EMPTY_CT, false, exitCondition);
      endOfLastSent += numPointsToSend;
      ++finalMotionID;
    }
 
    // Set up the interrupt frequency period
    controller->SyncInterruptPeriodSet(10); // this generates an interrupt every x cycles of a 1KHz sample rate
    // With our timeslices of 1ms, this is 10*1ms=10ms
    controller->SyncInterruptEnableSet(true);
 
    controller->MotionHoldGateSet(motionHoldGate, false); // release the hold gate to start moving
 
    while (!exitCondition)
    {
      int32_t sampleRecieved = controller->SyncInterruptWait();    // see above for timing
      curMotionID = multiAxis->MotionIdExecutingGet();        // this takes an unspecified amount of time (non-rtos) so this is going to be called sometime after the interrupt gets handled.
      // There's an additional delay to retrieve the data as well.
      curMotionElementID = multiAxis->MotionElementIdExecutingGet();
 
      /*
      Each MovePT assigns a new MotionID for each call to a move update (with the bufferred points)
      Working under the assumption that each Buffer gets a new ID, send two (or several) smaller ones
      and when the change in IDs is greater than some number, send a new one
      */
 
      // change this logic to manage the number of buffered moves (and points) as appropiate
      // generate points as appropiate
      if (std::abs(finalMotionID - curMotionID) < 2)
      {
        // check end condition
        if (TOTAL_POINTS <= (endOfLastSent + BUFFER_SZ))
        {
          numPointsToSend = TOTAL_POINTS - endOfLastSent; // send the remaining points
          exitCondition = true;
        }
        multiAxis->MovePT(RSIMotionType::RSIMotionTypePT, &positions[0] + endOfLastSent * AXIS_COUNT, &times[0] + endOfLastSent, numPointsToSend, EMPTY_CT, false, exitCondition);
 
        printf("MotionID %d\nEnd of Last Sent %d\nElement ID %d\nNum to Send %d\nIs Done %s\n===========================================\n\n",
          curMotionID, endOfLastSent, curMotionElementID, numPointsToSend, exitCondition ? "yes" : "no");
 
        endOfLastSent += numPointsToSend;
        ++finalMotionID;
      }
    }
    printf("Updates Done. Waiting to finish motion.\n");
    multiAxis->MotionDoneWait();
    printf("Motion Complete. Final Motion ID: %d\tFinal Element ID %d\n", multiAxis->MotionIdExecutingGet(), multiAxis->MotionElementIdExecutingGet());
 
    multiAxis->EStopAbort();
  }
  catch (RsiError const& err) {
    printf("\n%s\n", err.text);
    return -1;
  }
  controller->Delete();
  return 0;
}