DifferenceOfPositionUserLimit.cpp

 
#include <map>
 
#include "SampleAppsHelper.h" // Import our helper functions.
#include "rsi.h"              // Import our RapidCode Library.
 
using namespace RSI::RapidCode; // Import our RapidCode namespace
 
// Map of the RSIState enum to string
static const std::map<RSIState, std::string> RSIStateMap = {
    {RSIState::RSIStateIDLE, "RSIStateIDLE"},
    {RSIState::RSIStateMOVING, "RSIStateMOVING"},
    {RSIState::RSIStateSTOPPING, "RSIStateSTOPPING"},
    {RSIState::RSIStateSTOPPED, "RSIStateSTOPPED"},
    {RSIState::RSIStateSTOPPING_ERROR, "RSIStateSTOPPING_ERROR"},
    {RSIState::RSIStateERROR, "RSIStateERROR"}
};
 
int main()
{
  // Print a start message to indicate that the sample app has started
  const std::string SAMPLE_APP_NAME = "Math Blocks: Difference of Position User Limit";
  SampleAppsHelper::PrintHeader(SAMPLE_APP_NAME);
 
  /* CONSTANTS */
  // *NOTICE* The following constants must be configured before attempting to run with hardware.
  const int NUM_AXES = 2;   // The number of axes to configure
  const int AXIS_X_INDEX = 0; // The index of the first axis
  const int AXIS_Y_INDEX = 1; // The index of the second axis
 
  // User Limit Configuration
  const double MAX_POSITION_DIFFERENCE = 0.5;   // the maximum position difference before the user limit triggers
  const RSIAction USER_LIMIT_ACTION = RSIAction::RSIActionABORT;    // the action to take when the user limit is triggered
  const int USER_LIMIT_DURATION = 0;    // the time delay before the action is executed after the User Limit has triggered
 
  // Motion Parameters
  const double RELATIVE_POSITION = 2 * MAX_POSITION_DIFFERENCE;   // the relative position to move the axes
  const double VELOCITY = 1;                                      // the velocity to move the axes
  const double ACCELERATION = 10;                                 // the acceleration of the axes movement
  const double DECELERATION = 10;                                 // the deceleration of the axes movement
  const double JERK_PCT = 0;                                      // the jerk percentage of the axes movement
 
  /* RAPIDCODE INITIALIZATION */
  // Create the controller
  MotionController::CreationParameters params = SampleAppsHelper::GetCreationParameters();
  MotionController *controller = MotionController::Create(&params);
 
  // Variables to store initial object counts to restore later (starts at -1 to indicate it has not been set)
  int initialUserLimitCount = -1;
  int initialMathBlockCount = -1;
 
  // 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);
 
    // Save initial object counts to restore later
    initialUserLimitCount = controller->UserLimitCountGet();
    initialMathBlockCount = controller->MathBlockCountGet();
 
    /* SAMPLE APP BODY */
 
    /* Configure the controller object counts */
    // Normally you would set the number of axes here, but for samples that is handled in the SampleAppsHelper::SetupController function
    // controller->AxisCountSet(NUM_AXES); 
 
    // Each axis has a motion object associated with it, so we need to add one more motion object than axes
    const int multiAxisIndex = controller->AxisCountGet();
    controller->MotionCountSet(multiAxisIndex + 1);
 
    const int userLimitIndex = initialUserLimitCount;
    controller->UserLimitCountSet(initialUserLimitCount + 1);
 
    const int mathBlockIndex = initialMathBlockCount;
    controller->MathBlockCountSet(initialMathBlockCount + 1);
    
    // Get the axes
    Axis *axisX = controller->AxisGet(AXIS_X_INDEX);
    SampleAppsHelper::CheckErrors(axisX);
 
    Axis *axisY = controller->AxisGet(AXIS_Y_INDEX);
    SampleAppsHelper::CheckErrors(axisY);
 
    // Configure a multiaxis for the two axes
    MultiAxis *multiAxis = controller->MultiAxisGet(multiAxisIndex);
    SampleAppsHelper::CheckErrors(multiAxis);
 
    multiAxis->AxisRemoveAll();
    multiAxis->AxisAdd(axisX);
    multiAxis->AxisAdd(axisY);
 
    multiAxis->Abort(); // make sure the multiaxis is not moving
    multiAxis->ClearFaults();
 
    // Read the configuration of the MathBlock
    MotionController::MathBlockConfig mathBlockConfig = controller->MathBlockConfigGet(mathBlockIndex);
 
    // Determine which axis address type to use based on the config USE_HARDWARE flag
    RSIAxisAddressType INPUT_AXIS_ADDRESS_TYPE = (SampleAppsConfig::USE_HARDWARE) ? (RSIAxisAddressType::RSIAxisAddressTypeACTUAL_POSITION)
                                                                                  : (RSIAxisAddressType::RSIAxisAddressTypeCOMMAND_POSITION);
 
    // Configure the MathBlock to subtract the position of the second axis from the position of the first axis
    mathBlockConfig.InputAddress0 = axisX->AddressGet(INPUT_AXIS_ADDRESS_TYPE);
    mathBlockConfig.InputDataType0 = RSIDataType::RSIDataTypeDOUBLE;
    mathBlockConfig.InputAddress1 = axisY->AddressGet(INPUT_AXIS_ADDRESS_TYPE);
    mathBlockConfig.InputDataType1 = RSIDataType::RSIDataTypeDOUBLE;
    mathBlockConfig.ProcessDataType = RSIDataType::RSIDataTypeDOUBLE;
    mathBlockConfig.Operation = RSIMathBlockOperation::RSIMathBlockOperationSUBTRACT;
 
    // Set the MathBlock configuration
    controller->MathBlockConfigSet(mathBlockIndex, mathBlockConfig);
 
    // Wait a sample so we know the RMP is now processing the newly configured MathBlocks
    controller->SampleWait(1);
    std::cout << "MathBlock configured to subtract the position of the second axis from the position of the first axis." << std::endl;
 
    // Get the address of the MathBlock's ProcessValue to use in the UserLimit
    uint64_t mathBlockProcessValueAddress =
        controller->AddressGet(RSIControllerAddressType::RSIControllerAddressTypeMATHBLOCK_PROCESS_VALUE, mathBlockIndex);
 
    // Configure the UserLimit to trigger when the absolute position difference is greater than MAX_POSITION_DIFFERENCE
    controller->UserLimitConditionSet(
        userLimitIndex, 0, RSIUserLimitLogic::RSIUserLimitLogicABS_GT, mathBlockProcessValueAddress, MAX_POSITION_DIFFERENCE
    );
 
    // Set the UserLimit action to abort motion (Note: since the axes are in a multiaxis, the other axis will also be aborted)
    controller->UserLimitConfigSet(
        userLimitIndex, RSIUserLimitTriggerType::RSIUserLimitTriggerTypeSINGLE_CONDITION, USER_LIMIT_ACTION, AXIS_X_INDEX,
        USER_LIMIT_DURATION
    );
    std::cout << "UserLimit configured to trigger when the absolute position difference is greater than " << MAX_POSITION_DIFFERENCE
              << " and abort motion." << std::endl;
 
    // Command motion to trigger the UserLimit
    std::cout << "Moving the axes to trigger the UserLimit..." << std::endl;
    axisX->AmpEnableSet(true);                                                              // Enable the motor.
    axisX->MoveRelative(RELATIVE_POSITION, VELOCITY, ACCELERATION, DECELERATION, JERK_PCT); // Move the axis to trigger the UserLimit.
    axisX->MotionDoneWait();
 
    // Disable the motor and the UserLimit
    axisX->AmpEnableSet(false); // Disable the motor.
    controller->UserLimitDisable(userLimitIndex);
 
    // The motion should have been aborted and both axes should be in an error state
    if ((axisX->StateGet() == RSIState::RSIStateERROR) && (axisY->StateGet() == RSIState::RSIStateERROR))
    {
      std::cout << "Both axes are in the error state after the UserLimit triggered (This is the intended behavior)." << std::endl;
      exitCode = 0;
    }
    else
    {
      std::cout << "Error: The axes should be in an error state after the UserLimit triggers, but they are not." << std::endl;
      std::cout << "First Axis State: " << RSIStateMap.at(axisX->StateGet()) << std::endl;
      std::cout << "Second Axis State: " << RSIStateMap.at(axisY->StateGet()) << std::endl;
      exitCode = -1;
    }
  }
  catch (const std::exception &ex)
  {
    std::cerr << ex.what() << std::endl;
    exitCode = -1;
  }
  // Restore the object counts to the original values
  if (initialUserLimitCount != -1) { controller->UserLimitCountSet(initialUserLimitCount); }
  if (initialMathBlockCount != -1) { controller->MathBlockCountSet(initialMathBlockCount); }
  // Note: The axis and motion counts are handled by the SampleAppsHelper::Cleanup function
 
  // 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;
}