Update Buffer Points

Update Buffer Points sample application.

This application is designed to assist in managing the buffer in streaming motion. The behavior of continuing streaming motion (with the parameter final=false) will increment the MotionID (retrieved by calling MotionIdExecutingGet()). It will also reset the motion element ID to 0 (retrieved with MotionElementIdExecutingGet()).

The number sent points needs to be managed, somehow. This sample does so by using a fixed buffer size and keeping at least one buffer associated with a Motion ID queued. This should be changed as appropiate.

Precondition

This sample code presumes that the user has set the tuning paramters(PID, PIV, etc.) prior to running this program so that the motor can rotate in a stable manner.

Warning

This is a sample program to assist in the integration of the RMP motion controller with your application. It may not contain all of the logic and safety features that your application requires.

 
#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;
}