Math Blocks

Table of Contents

• 🔹 What are Math Blocks?
  • Supported Operations
• 🔹 Why use Math Blocks?
• 🔹 Math Block Structure
• 📜 Sample Code
  • User Limit on Difference of Position Between Two Axes
  • Calculate Acceleration From Velocity

Employ Math Blocks in RMP firmware for real-time mathematical operations on data, crucial for creating advanced control systems with operations ranging from basic arithmetic to complex bitwise calculations.

🔹 What are Math Blocks?

Math Blocks are a powerful feature within the RMP firmware, designed to facilitate real-time mathematical operations on data. These blocks are integral for developing sophisticated control systems, enabling calculations with the assurance of real-time execution, which is crucial for time-sensitive applications.

Every sampling interval, the RMP firmware evaluates each Math Block, allowing for a series of bitwise and mathematical operations on data inputs.

Supported Operations

Math Blocks support a variety of operations, categorized as follows:

Mathematical

• Addition
• Subtraction
• Multiplication
• Division
• Square Root
• Square Root of Sum of Squares

Bitwise

• OR
• AND
• XOR
• SHIFT

🔹 Why use Math Blocks?

Math Blocks are invaluable for data manipulation in real-time scenarios. For instance, calculating an axis's acceleration from velocity data, or determining the ratio between the encoder positions of two axes, are tasks where Math Blocks excel.

Math Blocks can be especially useful when integrated with User Limits to enhance system safety and efficiency. For example, a Math Block could compute the real-time positional difference between two axes. Coupled with a User Limit, the system could automatically halt operations if this difference exceeds a predefined threshold, preventing potential damage or inaccuracies.

🔹 Math Block Structure

📜 Sample Code

Learn how to use Math Blocks to perform real-time mathematical operations on data in the RMP firmware every sample.

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.

User Limit on Difference of Position Between Two Axes

MathBlock Operation SUBTRACT: First Axis Position - Second Axis Position Output: none

• C#

  /* This sample app demonstrates configuring a MathBlock to calculate the difference of position between two axes
     and using that MathBlock process value in a UserLimit to trigger an abort action when the difference exceeds a threshold.
     This sample app requires at least 2 Axis so that positions can be read.
  */
   
  using RSI.RapidCode; // RSI.RapidCode.dotNET;
   
  Console.WriteLine("📜 MathBlock: Difference of Position UserLimit");
   
  // set sample config params
  const int INDEX_ZERO = 0;
  const RSIDataType DOUBLE = RSIDataType.RSIDataTypeDOUBLE;
  const RSIMathBlockOperation SUBTRACT = RSIMathBlockOperation.RSIMathBlockOperationSUBTRACT;
   
  // get rmp controller
  MotionController controller = MotionController.Get();
   
  try
  {
      Helpers.CheckErrors(controller);
   
      // set mathblock & userlimit counts before any RapidCodeObject get/create other than the controller
      controller.MathBlockCountSet(1);
      controller.UserLimitCountSet(1);
   
      // get & configure both axes
      Axis axis0 = controller.AxisGet(Constants.AXIS_0_INDEX);
      Helpers.CheckErrors(axis0);
      axis0.ErrorLimitActionSet(RSIAction.RSIActionNONE);
      axis0.Abort();
      axis0.ClearFaults();
      controller.SampleWait(10);
      axis0.PositionSet(0);
   
      Axis axis1 = controller.AxisGet(Constants.AXIS_1_INDEX);
      Helpers.CheckErrors(axis1);
      axis1.ErrorLimitActionSet(RSIAction.RSIActionNONE);
      axis1.Abort();
      axis1.ClearFaults();
      controller.SampleWait(10);
      axis1.PositionSet(0);
   
      // set sample addresses
      ulong axis0PositionAddr = axis0.AddressGet(RSIAxisAddressType.RSIAxisAddressTypeCOMMAND_POSITION);
      ulong axis1PositionAddr = axis1.AddressGet(RSIAxisAddressType.RSIAxisAddressTypeCOMMAND_POSITION);
      ulong mbProcessValueAddr = controller.AddressGet(RSIControllerAddressType.RSIControllerAddressTypeMATHBLOCK_PROCESS_VALUE, INDEX_ZERO);
   
      // MathBlock config & set (mb.value = axis0.position - axis1.position)
      var mbConfig = controller.MathBlockConfigGet(INDEX_ZERO);
      mbConfig.Operation       = SUBTRACT;
      mbConfig.InputAddress0   = axis0PositionAddr;  // input0: axis0 command position
      mbConfig.InputAddress1   = axis1PositionAddr;  // input1: axis1 command position
      mbConfig.InputDataType0  = DOUBLE;
      mbConfig.InputDataType1  = DOUBLE;
      mbConfig.ProcessDataType = DOUBLE;
   
      controller.MathBlockConfigSet(INDEX_ZERO, mbConfig);
   
      // wait 1 sample for MathBlock config to take effect
      controller.SampleWait(1);
   
      // set UserLimit condition to trigger when absolute position difference exceeds threshold
      controller.UserLimitConditionSet(
          number:           INDEX_ZERO,
          conditionNumber:  0,
          logic:            RSIUserLimitLogic.RSIUserLimitLogicABS_GT,
          addressOfDouble:  mbProcessValueAddr,
          limitValueDouble: 5 * Constants.AXIS_0_USER_UNITS);  // 5 revolutions (values in firmware are not scaled by user units)
          
      // set UserLimit to abort when condition above is met     
      controller.UserLimitConfigSet(
          number:      INDEX_ZERO,
          triggerType: RSIUserLimitTriggerType.RSIUserLimitTriggerTypeSINGLE_CONDITION,
          action:      RSIAction.RSIActionABORT,
          actionAxis:  Constants.AXIS_0_INDEX,
          duration:    0);
   
      // move axis0 to trigger the UserLimit
      axis0.AmpEnableSet(true);
      axis0.MoveRelative(
          relativePosition: 10,  // revolutions
          vel:     1,
          accel:   1,
          decel:   1,
          jerkPct: 0);
      axis0.MotionDoneWait();
   
      // get results
      var axis0State = axis0.StateGet();
   
      // print results
      Console.WriteLine($"Axis 0 state: {axis0State} (expected: {RSIState.RSIStateERROR})");
   
      // verify userlimit triggered abort action
      if (axis0State != RSIState.RSIStateERROR)
          throw new Exception("❌ UserLimit did not trigger correctly - both axes should be in error state.");
   
  }
  // handle errors as needed
  finally
  {
      controller.UserLimitDisable(INDEX_ZERO);  // disable user limit
      controller.Delete();                      // dispose
  }

• C++

    /* 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();

Calculate Acceleration From Velocity

MathBlock 0 Operation SUBTRACT: Previous command velocity - current command velocity Output: none
MathBlock 1 Operation MULTIPLY: Current command velocity * 1.0 Output: write to UserBuffer
This sample code demonstrates how to use a MathBlock to calculate the difference of in position between two axes and trigger a UserLimit when the difference exceeds a certain value.

• C#

  /* This sample app demonstrates configuring MathBlocks to calculate acceleration from velocity.
     It uses one MathBlock to subtract the previous velocity from the current velocity to get the velocity delta,
     and a second MathBlock to store the current velocity as the previous velocity for the next calculation.
     The calculated acceleration is then derived from the velocity delta and sample rate.
  */
   
  using RSI.RapidCode; // RSI.RapidCode.dotNET;
   
  Console.WriteLine("📜 MathBlock: Calculate Acceleration from Velocity");
   
  // set sample config params
  const int MB_COUNT = 2;
  const int MB1 = 0;            // mathblock 1 index: subtract current velocity - previous velocity
  const int MB2 = 1;            // mathblock 2 index: store current velocity as previous velocity
  const double VELOCITY = 1.0;
  const double ACCELERATION = 0.0123;
  const RSIDataType DOUBLE = RSIDataType.RSIDataTypeDOUBLE;
  const RSIMathBlockOperation SUBTRACT = RSIMathBlockOperation.RSIMathBlockOperationSUBTRACT;
  const RSIMathBlockOperation MULTIPLY = RSIMathBlockOperation.RSIMathBlockOperationMULTIPLY;
   
  // get RMP controller
  MotionController controller = MotionController.Get();
      
  try
  {
      Helpers.CheckErrors(controller);
   
      // set mathblock count before any RapidCodeObject get/create other than the controller
      controller.MathBlockCountSet(MB_COUNT);
   
      // get & configure axis 
      Axis axis = controller.AxisGet(Constants.AXIS_0_INDEX);
      Helpers.CheckErrors(axis);
      axis.ErrorLimitActionSet(RSIAction.RSIActionNONE);
      axis.Abort();
      axis.ClearFaults();
      controller.SampleWait(10);
      axis.PositionSet(0);
   
      // set sample addresses
      ulong userBuffer0Addr = controller.AddressGet(RSIControllerAddressType.RSIControllerAddressTypeUSER_BUFFER, 0);
      ulong previousVelocityAddr = controller.AddressGet(RSIControllerAddressType.RSIControllerAddressTypeMATHBLOCK_PROCESS_VALUE, MB2);
      ulong currentVelocityAddr = axis.AddressGet(RSIAxisAddressType.RSIAxisAddressTypeCOMMAND_VELOCITY);
   
      // write 1 to user buffer index 0 for MB2 multiplication
      controller.MemoryDoubleSet(userBuffer0Addr, 1);
   
      // MathBlock 1 config: subtract current velocity - previous velocity
      var mbConfig1 = controller.MathBlockConfigGet(MB1);
      mbConfig1.Operation       = SUBTRACT;
      mbConfig1.InputAddress0   = currentVelocityAddr;   // input0: current command velocity
      mbConfig1.InputAddress1   = previousVelocityAddr;  // input1: previous velocity (from 2nd MathBlock)  
      mbConfig1.InputDataType0  = DOUBLE;
      mbConfig1.InputDataType1  = DOUBLE;
      mbConfig1.ProcessDataType = DOUBLE;
   
      // MathBlock 2 config: multiply current velocity by 1 to store previous velocity
      var mbConfig2 = controller.MathBlockConfigGet(MB2);
      mbConfig2.Operation       = MULTIPLY;
      mbConfig2.InputAddress0   = currentVelocityAddr;  // input0: current command velocity 
      mbConfig2.InputAddress1   = userBuffer0Addr;      // input1: constant "1" from user buffer
      mbConfig2.InputDataType0  = DOUBLE;
      mbConfig2.InputDataType1  = DOUBLE;
      mbConfig2.ProcessDataType = DOUBLE;
   
      // set MathBlock configurations
      controller.MathBlockConfigSet(MB1, mbConfig1);
      controller.MathBlockConfigSet(MB2, mbConfig2);
   
      // wait 1 sample for MathBlock config to take effect
      controller.SampleWait(1);
   
      // move axis with small acceleration to verify MathBlock calculation
      axis.AmpEnableSet(true);
      axis.MoveVelocity(VELOCITY, ACCELERATION);
      controller.SampleWait(10);
   
      // get latest sample velocity delta from MathBlock 1
      double latestVelocityDelta = controller.MathBlockProcessValueGet(MB1).Double;
   
      // reduce velocity back to 0
      axis.MoveVelocity(0, ACCELERATION);
      axis.MotionDoneWait();
   
      // get results
      double sampleRate = controller.SampleRateGet();
      double userUnits = axis.UserUnitsGet();
      double calculatedAcceleration = latestVelocityDelta * Math.Pow(sampleRate, 2) / userUnits;
   
      // rounding to 8 decimals matches encoder resolution for most systems.
      // this is more precise than typical hardware needs.
      double calculatedAccelerationRounded = Math.Round(calculatedAcceleration, 8);
   
      // print results
      Console.WriteLine($"Acceleration: {calculatedAccelerationRounded} (expected: {ACCELERATION})");
   
      // verify accuracy
      if (ACCELERATION != calculatedAccelerationRounded)
          throw new Exception("❌ MathBlock result is outside accepted tolerance.");
  }
  // handle errors as needed
  finally
  {
      controller.Delete();  // dispose
  }

• C++

    /* 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 = 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 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
      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); 
   
      // 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);
      SampleAppsHelper::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 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();