PVTmotionMultiAxis.cpp

 
#include "rsi.h"                // Import our RapidCode Library. 
#include "SampleAppsHelper.h"    // Import our SampleApp helper functions.
#include "SampleApps.h"
 
using namespace RSI::RapidCode;                     // Import the RapidCode namespace
 
int PVTmotionMultiAxis::Run()
{
  /* CONSTANTS */
  // *NOTICE* The following constants must be configured before attempting to run with hardware.
 
  // Axis configuration parameters
  const int AXIS_COUNT = 2;
  const int AXIS_X = 0;
  const int AXIS_Y = 1;
 
  const double USER_UNITS = 1048576;
 
  // Motion parameters
  const int POINTS = (3000);   //total points used
  const double TIME_SLICE = (0.001);   //each point processed within 1ms
 
  // 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;
 
  // The rmpPath only needs to be set if the project directory is different than the rapid setup directory.
  // *NOTICE* When setting the rmpPath, be sure to uncomment the rmpPath arguement in MotionController::CreateFromSoftware() in the Run() function.
  const char rmpPath[] = "C:\\RSI\\X.X.X\\";            // Insert the path location of the RMP.rta (usually the RapidSetup folder)
 
  /* SAMPLE APP BODY */
 
  // Initialize MotionController class.
  MotionController *controller = MotionController::CreateFromSoftware(/*rmpPath*/); // NOTICE: Uncomment "rmpPath" if project directory is different than rapid setup directory.
  SampleAppsHelper::CheckErrors(controller);                                           // [Helper Function] Check that the axis has been initialize correctly. 
 
  // Setup the controller for the appropriate hardware configuration.
  if (USE_HARDWARE)
  {
    SampleAppsHelper::SetupControllerForHardware(controller);
  }
  else
  {
    SampleAppsHelper::SetupControllerForPhantoms(controller, AXIS_COUNT, { AXIS_X, AXIS_Y });
  }
 
  try
  {
    // enable one MotionSupervisor for the MultiAxis
    controller->MotionCountSet(controller->AxisCountGet() + 1);
 
    // Get Axis X and Y respectively.
    Axis* axisX = controller->AxisGet(AXIS_X);
    SampleAppsHelper::CheckErrors(axisX);
    axisX->UserUnitsSet(USER_UNITS);
    axisX->ErrorLimitActionSet(RSIAction::RSIActionNONE);
 
    Axis* axisY = controller->AxisGet(AXIS_Y);
    SampleAppsHelper::CheckErrors(axisY);
    axisY->UserUnitsSet(USER_UNITS);
    axisY->ErrorLimitActionSet(RSIAction::RSIActionNONE);
 
    // Initialize a MultiAxis, using the last MotionSupervisor.
    MultiAxis* multiAxisXY = controller->MultiAxisGet(controller->MotionCountGet() - 1);
    SampleAppsHelper::CheckErrors(multiAxisXY);
    multiAxisXY->AxisAdd(axisX);
    multiAxisXY->AxisAdd(axisY);
 
    long radius = 3;                     //radius of circle
 
    double PI = 3.14159265358979323;        //variable used in equation to convert degrees to radians
 
    double degrees = 90.00 / (POINTS);      //angle between each point. Used in the equation below.
 
    double position[POINTS * AXIS_COUNT];   //defining size of position array
    double vel[POINTS * AXIS_COUNT];        //defining size of velocity array
    double time[POINTS];                    //defining size of time array
 
    for (int i = 0, x = 0, y = 1; i < POINTS; i++)
    {
        position[x] = (radius*cos((i + 1)*degrees*PI / 180.0));
        position[y] = (radius*sin((i + 1)*degrees*PI / 180.0));
 
        x = x + 2;
        y = y + 2;
 
        time[i] = TIME_SLICE;
    }
 
    for (int i = 0, x = 0, y = 1; i < (POINTS - 1); i++)
    {
        vel[x] = (position[x + 2] - position[x]) / TIME_SLICE;
        vel[y] = (position[y + 2] - position[y]) / TIME_SLICE;
 
        x = x + 2;
        y = y + 2;
    }
 
    //Final two points (X Axis Final Vel, Y Axis Final Vel) need to be set to 0.
    vel[(POINTS * 2) - 2] = 0;  //X Axis
    vel[(POINTS * 2) - 1] = 0;  //Y Axis
 
    // Set the initial position of the axes.
    axisX->PositionSet(radius);
    axisY->PositionSet(0);
 
    multiAxisXY->Abort();
    multiAxisXY->ClearFaults();
    multiAxisXY->AmpEnableSet(true);
 
    multiAxisXY->MovePVT(position, vel, time, POINTS, -1, false, true);
    multiAxisXY->MotionDoneWait();
 
    multiAxisXY->AmpEnableSet(false);
  }
  catch (RsiError const& err)
  {
      printf("\n%s\n", err.text);
      return -1;
  }
  controller->Delete();                                   // Delete the controller as the program exits to ensure memory is deallocated in the correct order.
  return 0;
}