Motion: Path

Path Motion sample application.

This sample app will command a path motion for two axis with different points and desired velocity, acceleration and deceleration specified by user.

Different positions have to be secified within 'PathListStart' and 'PathListEnd' using function 'PathLineAdd' where the coordinates are declared in line_A, line_B and etc.

Function 'PathRatioSet(ratio)' is used to define different ratios between the two different drive's encoder resolution. As the name 'PathMotionStart' suggests will simply execute motion for axes.

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