IO: Math Blocks

Learn how to use Math Blocks for calculations and data processing.

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. We recommend that you wire an external hardware emergency stop (e-stop) button for safety when using our code sample apps. Doing so will help ensure the safety of you and those around you and will prevent potential injury or damage.

The sample apps assume that the system (network, axes, I/O) are configured prior to running the code featured in the sample app. See the Configuration page for more information.

In this page:

• 📜 Math Blocks: Calculate Acceleration from Velocity
• 📜 Math Blocks: Difference of Position User Limit

📜 Math Blocks: Calculate Acceleration from Velocity

Learn how to use Math Blocks to calculate acceleration from velocity data for motion analysis and control applications.

  /* 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
  const double VELOCITY = 1.0;       // the velocity to move the axis
  const double ACCELERATION = 0.123; // something small so we can check the MathBlock is working
 
  /* RAPIDCODE INITIALIZATION */
  // create the Controller
  MotionController::CreationParameters params = Config::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 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 config.h depending on the configuration
    Helpers::CheckErrors(controller);
    Config::SetupController(controller, NUM_AXES);
 
    // save initial object counts to restore later
    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 Config::SetupController function
    // controller->AxisCountSet(NUM_AXES); 
 
    // add the two MathBlocks needed for this sample
    const int subtractionMathBlockIndex = initialMathBlockCount;
    const int previousVelocityMathBlockIndex = initialMathBlockCount + 1;
    controller->MathBlockCountSet(initialMathBlockCount + 2);
 
    //  get the axis and make sure the axis is not moving and clear any faults
    Axis *axis = controller->AxisGet(AXIS_INDEX);
    Helpers::CheckErrors(axis);
    axis->Abort();
    axis->ClearFaults();
 
    // read the configuration of both MathBlocks
    MotionController::MathBlockConfig subtractionConfig = controller->MathBlockConfigGet(subtractionMathBlockIndex);
 
    // this index must be greater than the subtraction math block index, so the subtraction data is
    // one sample old
    MotionController::MathBlockConfig previousVelocityConfig = controller->MathBlockConfigGet(previousVelocityMathBlockIndex);
 
    // set the axis to use the command velocity as the input for the MathBlock
    RSIAxisAddressType INPUT_AXIS_ADDRESS_TYPE = RSIAxisAddressType::RSIAxisAddressTypeCOMMAND_VELOCITY;
 
    // configure the first MathBlock to subtract the previous velocity from the current velocity
    // current velocity:
    subtractionConfig.InputAddress0 = axis->AddressGet(INPUT_AXIS_ADDRESS_TYPE);
    subtractionConfig.InputDataType0 = RSIDataType::RSIDataTypeDOUBLE;
 
    // previous velocity: (as was calculated by the second MathBlock, so we use its ProcessValue)
    subtractionConfig.InputAddress1 =
        controller->AddressGet(RSIControllerAddressType::RSIControllerAddressTypeMATHBLOCK_PROCESS_VALUE, previousVelocityMathBlockIndex);
    subtractionConfig.InputDataType1 = RSIDataType::RSIDataTypeDOUBLE;
 
    subtractionConfig.ProcessDataType = RSIDataType::RSIDataTypeDOUBLE;
    subtractionConfig.Operation = RSIMathBlockOperation::RSIMathBlockOperationSUBTRACT;
 
    // write 1.0 to the first UserBuffer entry so the second MathBlock can use it for multiplication
    uint64_t userBufferAddr0 = controller->AddressGet(RSIControllerAddressType::RSIControllerAddressTypeUSER_BUFFER, 0);
    controller->MemoryDoubleSet(userBufferAddr0, 1.0);
 
    // configure the second MathBlock to multiply the current velocity by 1.0 (which we'll use for
    // the previous sample's velocity)
    previousVelocityConfig.InputAddress0 = axis->AddressGet(INPUT_AXIS_ADDRESS_TYPE);
    previousVelocityConfig.InputDataType0 = RSIDataType::RSIDataTypeDOUBLE;
    previousVelocityConfig.InputAddress1 = userBufferAddr0;
    previousVelocityConfig.InputDataType1 = RSIDataType::RSIDataTypeDOUBLE;
    previousVelocityConfig.ProcessDataType = RSIDataType::RSIDataTypeDOUBLE;
    previousVelocityConfig.Operation = RSIMathBlockOperation::RSIMathBlockOperationMULTIPLY;
 
    // set the MathBlock configurations
    controller->MathBlockConfigSet(subtractionMathBlockIndex, subtractionConfig);
    controller->MathBlockConfigSet(previousVelocityMathBlockIndex, previousVelocityConfig);
 
    // Wait a sample so we know the RMP is now processing the newly configured MathBlocks
    controller->SampleWait(1);
 
    // Set the axis to move with a very small acceleration so we can check the MathBlock is working
    axis->AmpEnableSet(true); // Enable the motor.
    axis->MoveVelocity(VELOCITY, ACCELERATION);
 
    // Wait several samples so we know the RMP is now processing the move command and accelerating
    controller->SampleWait(10);
 
    // Keep in mind firmware velocity is in counts per sample, so we need to convert to UserUnits
    // per second squared
    double calculatedVelocityDelta = controller->MathBlockProcessValueGet(subtractionMathBlockIndex).Double;
 
    // Reduce the velocity back to 0
    axis->MoveVelocity(0, ACCELERATION);
 
    axis->MotionDoneWait();    // Wait for the axis to finish moving.
    axis->AmpEnableSet(false); // Disable the motor.
 
    // Convert to UserUnits per second squared
    double calculatedAcceleration = calculatedVelocityDelta * controller->SampleRateGet() * controller->SampleRateGet() / axis->UserUnitsGet();
    std::cout << "Calculated acceleration from MathBlock: " << calculatedAcceleration << std::endl;
 
    // Check that the newly calculated acceleration is as expected
    if (std::abs(calculatedAcceleration - ACCELERATION) <= 0.000001)
    {
      std::cout << "The MathBlock is calculating the Axis' acceleration by subtracting previous velocity from current velocity." << std::endl;
      exitCode = 0;
    }
    else
    {
      std::cerr << "Error: The calculated acceleration does not match the expected value" << 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 (initialMathBlockCount != -1) { controller->MathBlockCountSet(initialMathBlockCount); }
  // Note: The axis count is handled by the Config::Cleanup function
 
  // clean up the controller and any other objects as needed
  Config::Cleanup(controller);
 
  // delete the controller as the program exits to ensure memory is deallocated in the correct order
  controller->Delete();

Source: calculate-acceleration-from-velocity.cpp

📜 Math Blocks: Difference of Position User Limit

Learn how to use Math Blocks to monitor position differences and implement user limits for advanced motion control and safety applications.

  /* 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 = Config::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 config.h depending on the configuration
    Helpers::CheckErrors(controller);
    Config::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 Config::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);
    Helpers::CheckErrors(axisX);
 
    Axis *axisY = controller->AxisGet(AXIS_Y_INDEX);
    Helpers::CheckErrors(axisY);
 
    // Configure a multiaxis for the two axes
    MultiAxis *multiAxis = controller->MultiAxisGet(multiAxisIndex);
    Helpers::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 = (Config::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 Config::Cleanup function
 
  // clean up the controller and any other objects as needed
  Config::Cleanup(controller);
 
  // delete the controller as the program exits to ensure memory is deallocated in the correct order
  controller->Delete();

Source: difference-of-position-user-limit.cpp