PointToPoint.cpp

 
#include <cmath>
 
#include "SampleAppsHelper.h" // Import our helper functions.
#include "rsi.h"              // Import our RapidCode Library.
 
using namespace RSI::RapidCode; // Import the RapidCode namespace
 
// Helper function to wait for the axis to reach velocity when performing a velocity move or move with final velocity
static void HelperAtVelocityWait(MotionController *controller, Axis *axis, const int timeout)
{
  // The number of samples to wait before checking if the axis is at velocity
  const int WAIT_SAMPLES = 100;
 
  int samplesWaited = 0;
  while (axis->StatusBitGet(RSIEventType::RSIEventTypeMOTION_AT_VELOCITY) == 0)
  {
    controller->SampleWait(WAIT_SAMPLES);
    samplesWaited += WAIT_SAMPLES;
    if (samplesWaited > timeout)
    {
      throw std::runtime_error("Timeout waiting for motion to reach velocity");
    }
  }
}
 
int main()
{
  // Print a start message to indicate that the sample app has started
  const std::string SAMPLE_APP_NAME = "Motion: Point-to-Point";
  SampleAppsHelper::PrintHeader(SAMPLE_APP_NAME);
 
  /* CONSTANTS */
  // *NOTICE* The following constants must be configured before attempting to run with hardware.
  const int NUM_AXES = 1; // The number of axes to configure
  const int AXIS_INDEX = 0; // The index of the axis to configure
 
  // Motion Parameters
  const double POSITION_0 = 0;       
  const double POSITION_1 = 0.5;     
  const double VELOCITY = 1;         
  const double ACCELERATION = 10;    
  const double DECELERATION = 10;    
  const double JERK_PERCENT = 50;    
  const double FINAL_VELOCITY = 0.5; 
 
  /* RAPIDCODE INITIALIZATION */
  // Create the controller
  MotionController::CreationParameters params = SampleAppsHelper::GetCreationParameters();
  MotionController *controller = MotionController::Create(&params);
 
  // Set the exit code to an error value.
  int exitCode = -1;  
  try   // Ensure that the controller is deleted if an error occurs.
  {
    // Prepare the controller as defined in SampleAppsHelper.h depending on the configuration
    SampleAppsHelper::CheckErrors(controller);
    SampleAppsHelper::SetupController(controller, NUM_AXES);
 
    /* SAMPLE APP BODY */
 
    /* SET CONTROLLER OBJECT COUNTS HERE*/
    // Normally you would set the number of axes here, but for samples that is handled in the SampleAppsHelper::SetupController function
    // controller->AxisCountSet(NUM_AXES); 
 
    // Get the axes
    Axis *axis = controller->AxisGet(AXIS_INDEX);
    SampleAppsHelper::CheckErrors(axis);
 
    // Specify a timeout for waiting for the motion to complete
    const int32_t TIMEOUT = 10000;
 
    // Clear faults and enable the motor
    axis->ClearFaults();
    axis->AmpEnableSet(true);
 
    /* Absolute (Trapezoidal) Motion */
    std::cout << "Absolute (Trapezoidal) Motion:" << std::endl;
    std::cout << "Moving to position: " << POSITION_1 << std::endl;
    axis->MoveTrapezoidal(POSITION_1, VELOCITY, ACCELERATION, DECELERATION);
    axis->MotionDoneWait(TIMEOUT);
    std::cout << "Motion Complete" << std::endl;
 
    std::cout << "Moving back to position: " << POSITION_0 << std::endl;
    axis->MoveTrapezoidal(POSITION_0, VELOCITY, ACCELERATION, DECELERATION);
    axis->MotionDoneWait(TIMEOUT);
    std::cout << "Motion Complete\n" << std::endl;
 
    /* Relative Motion */
    std::cout << "Relative Motion:" << std::endl;
    std::cout << "Moving to position: " << POSITION_1 << std::endl;
    axis->MoveRelative(POSITION_1 - POSITION_0, VELOCITY, ACCELERATION, DECELERATION, JERK_PERCENT);
    axis->MotionDoneWait(TIMEOUT);
    std::cout << "Motion Complete" << std::endl;
 
    std::cout << "Moving back to position: " << POSITION_0 << std::endl;
    axis->MoveRelative(POSITION_0 - POSITION_1, VELOCITY, ACCELERATION, DECELERATION, JERK_PERCENT);
    axis->MotionDoneWait(TIMEOUT);
    std::cout << "Motion Complete\n" << std::endl;
 
    /* SCurve Motion */
    std::cout << "SCurve Motion:" << std::endl;
    std::cout << "Moving to position: " << POSITION_1 << std::endl;
    axis->MoveSCurve(POSITION_1);
    axis->MotionDoneWait(TIMEOUT);
    std::cout << "Motion Complete" << std::endl;
 
    std::cout << "Moving back to position: " << POSITION_0 << std::endl;
    axis->MoveSCurve(POSITION_0);
    axis->MotionDoneWait(TIMEOUT);
    std::cout << "Motion Complete\n" << std::endl;
 
    /* SCurve Motion with Final Velocity */
    std::cout << "SCurve Motion with Final Velocity:" << std::endl;
    std::cout << "Moving to position: " << POSITION_1 << std::endl;
    std::cout << "Final Velocity: " << FINAL_VELOCITY << std::endl;
    axis->MoveSCurve(POSITION_1, VELOCITY, ACCELERATION, DECELERATION, JERK_PERCENT, FINAL_VELOCITY);
    HelperAtVelocityWait(controller, axis, TIMEOUT);
    std::cout << "Motion Complete" << std::endl;
 
    std::cout << "Moving back to position: " << POSITION_0 << std::endl;
    std::cout << "Final Velocity: " << 0 << std::endl;
    axis->MoveSCurve(POSITION_0, VELOCITY, ACCELERATION, DECELERATION, JERK_PERCENT, 0);
    axis->MotionDoneWait(TIMEOUT);
    std::cout << "Motion Complete\n" << std::endl;
 
    /* Velocity Move */
    std::cout << "Velocity Move:" << std::endl;
    std::cout << "Accelerating to velocity: " << VELOCITY << std::endl;
    axis->MoveVelocity(VELOCITY, ACCELERATION);
    HelperAtVelocityWait(controller, axis, TIMEOUT);
    std::cout << "Motion Complete" << std::endl;
 
    std::cout << "Decelerating to velocity: " << 0 << std::endl;
    axis->MoveVelocity(0, DECELERATION);
    HelperAtVelocityWait(controller, axis, TIMEOUT);
    std::cout << "Motion Complete" << std::endl;
 
    // Return to the original position
    axis->MoveTrapezoidal(POSITION_0, VELOCITY, ACCELERATION, DECELERATION);
    axis->MotionDoneWait(TIMEOUT);
 
    // Disable the motor
    axis->AmpEnableSet(false);
 
    exitCode = 0; // Set the exit code to success.
  }
  catch (const std::exception &ex)
  {
    std::cerr << ex.what() << std::endl;
    exitCode = -1;
  }
  // Clean up the controller and any other objects as needed
  SampleAppsHelper::Cleanup(controller);
 
  // Delete the controller as the program exits to ensure memory is deallocated in the correct order
  controller->Delete();
 
  // Print a message to indicate the sample app has finished and if it was successful or not
  SampleAppsHelper::PrintFooter(SAMPLE_APP_NAME, exitCode);
 
  return exitCode;
}