PathMotion.cpp

Attention

See the following Concept pages for a detailed explanation of this sample: Path Motion.

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. We recommend that you wire an external hardware emergency stop (e-stop) button for safety when using our code sample apps. Doing so will help ensure the safety of you and those around you and will prevent potential injury or damage.

The sample apps assume that the system (network, axes, I/O) are configured prior to running the code featured in the sample app. See the Configuration page for more information.

 
#include <iostream>
#include <fstream>
 
#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 PathMotion::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;
 
  // Default file path for recording the path followed by the axes
  const char* const PATH_MOTION_FILEPATH = "PathMotion.csv";
 
  // 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 */
 
  Axis* axisX;
  Axis* axisY;
  MultiAxis* multiAxisXY;
 
  double running_x = 0;
  double running_y = 0;
 
  double line_A[2] = { running_x, running_y };
  running_x += -0.5;
  double line_B[2] = { running_x, running_y };
  running_y += -0.5;
  double line_C[2] = { running_x, running_y };
 
  //curve down
  running_y += (-0.25);
  double arc_center_A[2] = { running_x,running_y };
  running_x += (0.25);
 
  //curve left
  running_x += -0.25;
  double arc_center_B[2] = { running_x,running_y };
  running_y += -0.25;
 
  //curve up
  running_y += 0.25;
  double arc_center_C[2] = { running_x,running_y };
  running_x += -0.25;
 
  //curve right
  running_x += 0.25;
  double arc_center_D[2] = { running_x,running_y };
  running_y += 0.25;
 
  running_x += -0.5;
  running_y += 0.5;
  double line_D[2] = { running_x, running_y };
 
  // Create and initialize RsiController class 
  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_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->PositionSet(0);
    axisX->HomeMethodSet(RSIHomeMethod::RSIHomeMethodCURRENT_POSITION);
    axisX->ErrorLimitActionSet(RSIAction::RSIActionNONE);
    axisX->AmpDisableActionSet(RSI::RapidCode::RSIMotorDisableAction::RSIMotorDisableActionNONE);
    axisX->SoftwareNegLimitActionSet(RSI::RapidCode::RSIAction::RSIActionNONE);
    axisX->SoftwarePosLimitActionSet(RSI::RapidCode::RSIAction::RSIActionNONE);
    axisX->UserUnitsSet(USER_UNITS);
    axisX->Abort();
 
    Axis* axisY = controller->AxisGet(AXIS_Y);
    SampleAppsHelper::CheckErrors(axisY);
    axisY->PositionSet(0);
    axisY->HomeMethodSet(RSIHomeMethod::RSIHomeMethodCURRENT_POSITION);
    axisY->ErrorLimitActionSet(RSIAction::RSIActionNONE);
    axisY->AmpDisableActionSet(RSI::RapidCode::RSIMotorDisableAction::RSIMotorDisableActionNONE);
    axisY->SoftwareNegLimitActionSet(RSI::RapidCode::RSIAction::RSIActionNONE);
    axisY->SoftwarePosLimitActionSet(RSI::RapidCode::RSIAction::RSIActionNONE);
    axisY->UserUnitsSet(USER_UNITS);
    axisY->Abort();
 
    // Initialize a MultiAxis, using the last MotionSupervisor.
    MultiAxis* multiAxisXY = controller->MultiAxisGet(controller->MotionCountGet() - 1);
    SampleAppsHelper::CheckErrors(multiAxisXY);
    multiAxisXY->AxisAdd(axisX);
    multiAxisXY->AxisAdd(axisY);
 
    // make sure all axes are enabled and ready
    multiAxisXY->Abort();
    multiAxisXY->ClearFaults();
    multiAxisXY->AmpEnableSet(true);
 
    axisX->Home();
    axisY->Home();
 
    multiAxisXY->AmpEnableSet(false);
 
    // set the trajectory info
    multiAxisXY->VectorVelocitySet(1.0 * USER_UNITS);
    multiAxisXY->VectorAccelerationSet(10.0 * USER_UNITS);
    multiAxisXY->VectorDecelerationSet(10.0 * USER_UNITS);
 
    // start path list
    double start_positions[2] = { axisX->CommandPositionGet(),axisY->CommandPositionGet() };
    multiAxisXY->PathListStart(start_positions);
 
    multiAxisXY->ClearFaults();
 
    // turn on blending (smooth corners)
    multiAxisXY->PathBlendSet(true);
 
    // Lines and arcs
    multiAxisXY->PathLineAdd(line_A);
    multiAxisXY->PathLineAdd(line_B);
    multiAxisXY->PathLineAdd(line_C);
    multiAxisXY->PathArcAdd(arc_center_A, -90.0);
    multiAxisXY->PathArcAdd(arc_center_B, -90.0);
    multiAxisXY->PathArcAdd(arc_center_C, -90.0);
    multiAxisXY->PathArcAdd(arc_center_D, -90.0);
    multiAxisXY->PathLineAdd(line_D);
 
    // end path list
    multiAxisXY->PathListEnd();
 
    // make sure all axes are enabled and ready
    multiAxisXY->ClearFaults();
    multiAxisXY->AmpEnableSet(true);
    // execute the motion
    multiAxisXY->PathMotionStart();
 
    // log positions
    std::ofstream myfile;
    myfile.open(PATH_MOTION_FILEPATH);
    myfile << "samples,x,y,\n";
 
    while (!multiAxisXY->MotionDoneGet()) {
 
      myfile << controller->SampleCounterGet() << "," << axisX->CommandPositionGet() << "," << axisY->CommandPositionGet() << "\n";
      controller->SampleWait(10);
    }
    // wait for motion to complete
    multiAxisXY->MotionDoneWait();
    multiAxisXY->AmpEnableSet(false);
    myfile.close();
    printf("\n%s\n", PATH_MOTION_FILEPATH);
  }
  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;
}