PathMotion.cpp

 
#include <iostream>
#include <fstream>
 
#include "rsi.h"                // Import our RapidCode Library. 
#include "HelperFunctionsCpp.h"    // Import our SampleApp helper functions.
#include "SampleApps.h"
 
using namespace RSI::RapidCode;
 
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();
  HelperFunctionsCpp::CheckErrors(controller);
 
  // Setup the controller for the appropriate hardware configuration.
  if (USE_HARDWARE)
  {
    HelperFunctionsCpp::SetupControllerForHardware(controller);
  }
  else
  {
    HelperFunctionsCpp::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);
    HelperFunctionsCpp::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);
    HelperFunctionsCpp::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);
    HelperFunctionsCpp::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;
}