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()
{
  const std::string SAMPLE_APP_NAME = "Math Blocks: Difference of Position User Limit";
 
  // Print a start message to indicate that the sample app has started
  SampleAppsHelper::PrintHeader(SAMPLE_APP_NAME);
 
  // *NOTICE* The following constants must be configured before attempting to run with hardware.
 
  // User Limit Configuration
  const double MAX_POSITION_DIFFERENCE = 0.5 * SampleAppsConfig::AXIS_X_USER_UNITS; // 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);
 
  int exitCode = -1; // Set the exit code to an error value.
  try                // Ensure that the controller is deleted if an error occurs.
  {
    SampleAppsHelper::CheckErrors(controller);
 
    // Prepare the controller and axes as defined in SampleAppsHelper.h
    SampleAppsHelper::SetupController(controller);
 
    /* SAMPLE APP BODY */
 
    /* Configure the controller object counts */
    controller->MathBlockCountSet(SampleAppsConfig::MATH_BLOCK_COUNT + 1);
    controller->MotionCountSet(SampleAppsConfig::AXIS_COUNT + 1);
    controller->UserLimitCountSet(SampleAppsConfig::USER_LIMIT_COUNT + 1);
 
    // Get the axes
    Axis *axisX = controller->AxisGet(SampleAppsConfig::AXIS_X_INDEX);
    SampleAppsHelper::CheckErrors(axisX);
 
    Axis *axisY = controller->AxisGet(SampleAppsConfig::AXIS_Y_INDEX);
    SampleAppsHelper::CheckErrors(axisY);
 
    // Configure a multiaxis for the two axes
    MultiAxis *multiAxis = controller->MultiAxisGet(SampleAppsConfig::AXIS_COUNT);
    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
    const int MATHBLOCK_INDEX = SampleAppsConfig::MATH_BLOCK_COUNT;
    MotionController::MathBlockConfig mathBlockConfig = controller->MathBlockConfigGet(MATHBLOCK_INDEX);
 
    // 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(MATHBLOCK_INDEX, 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, MATHBLOCK_INDEX);
 
    // Configure the UserLimit to trigger when the absolute position difference is greater than MAX_POSITION_DIFFERENCE
    const int USER_LIMIT_INDEX = SampleAppsConfig::USER_LIMIT_COUNT;
    controller->UserLimitConditionSet(
        USER_LIMIT_INDEX, 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(
        USER_LIMIT_INDEX, RSIUserLimitTriggerType::RSIUserLimitTriggerTypeSINGLE_CONDITION, USER_LIMIT_ACTION, SampleAppsConfig::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(USER_LIMIT_INDEX);
 
    // 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;
  }
  // 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;
}