Math Blocks

Learn how to create and use Math Blocks in C#.

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 Block: Calculate Acceleration from Velocity
• 📜 Math Block: Difference of Position User Limit
• 📜 Math Block: PDO Copy

📜 Math Block: Calculate Acceleration from Velocity

Learn how to configure Math Blocks 📖 to calculate the acceleration based on the change in velocity over time.

/* 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
}

📜 Math Block: Difference of Position User Limit

Learn how to configure Math Blocks 📖 to set a value in memory and configure a User Limits 📖 to monitor and trigger an action based on it.

/* 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
}

📜 Math Block: PDO Copy

Learn how to configure Math Blocks 📖 to copy a PDO value from one axis to another with a bias offset.

/*  This sample app demonstrates configuring a MathBlock to copy a PDO value from one axis to another with a bias offset.
    This sample app requires at least 2 real Axis so that PDOs can be read and written.
*/
 
using RSI.RapidCode; // RSI.RapidCode.dotNET;
 
Console.WriteLine("📜 MathBlock: PDO Copy");
 
// set sample config params
const int MB_INDEX = 0;
const int BIAS = 8000;
const RSIDataType INT32 = RSIDataType.RSIDataTypeINT32;
const RSIMathBlockOperation ADD = RSIMathBlockOperation.RSIMathBlockOperationADD;
 
// get RMP controller
MotionController controller = MotionController.Get();
 
try
{
    Helpers.CheckErrors(controller);
 
    // check is network is started
    if (controller.NetworkStateGet() != RSINetworkState.RSINetworkStateOPERATIONAL)
    {
        Console.WriteLine("Network not started. Please start it before running this app.");
        return;
    }
 
    // set mathblock count before any RapidCodeObject get/create other than the controller
    controller.MathBlockCountSet(1);
 
    // get & configure axis 0
    Axis axis0 = controller.AxisGet(Constants.AXIS_0_INDEX);
    Helpers.CheckErrors(axis0);
    axis0.ErrorLimitActionSet(RSIAction.RSIActionNONE);
    axis0.Abort();
    axis0.ClearFaults();
    controller.SampleWait(10);
 
    // get & configure axis 1
    Axis axis1 = controller.AxisGet(Constants.AXIS_1_INDEX);
    Helpers.CheckErrors(axis1);
    axis1.ErrorLimitActionSet(RSIAction.RSIActionNONE);
    axis1.Abort();
    axis1.ClearFaults();
    controller.SampleWait(10);
 
    // check axis are real and not phantom
    if (!axis0.NetworkNode.Exists() || !axis1.NetworkNode.Exists())
    {
        Console.WriteLine("One or both axes are phantom (not real). Please use real axes for this sample app.");
        return;
    }
 
    // write BIAS value to user buffer index 0
    // if you want a dynamic bias, you can write to this address at any time or use the process value of another MathBlock instead
    ulong userBufferAddr0 = controller.AddressGet(RSIControllerAddressType.RSIControllerAddressTypeUSER_BUFFER, 0);
    controller.MemorySet(userBufferAddr0, BIAS);
 
    // configure the MathBlock to copy a PDO value from axis 0 to a PDO override value on axis 1 with a BIAS offset
    // here we are using the actual torque of axis 0 and the target torque of axis 1, but these can be replaced with whatever PDOs you want
    // by changing the index values to the desired PDOs
    uint torqueActualIndex = axis0.NetworkIndexGet(RSINetworkIndexType.NetworkIndexTypeTORQUE_ACTUAL_INDEX);
    ulong torqueActualAddress = controller.NetworkInputAddressGet((int)torqueActualIndex);
 
    uint targetTorqueIndex = axis1.NetworkIndexGet(RSINetworkIndexType.NetworkIndexTypeTARGET_TORQUE_INDEX);
    ulong targetTorqueAddress = controller.NetworkOutputAddressGet((int)targetTorqueIndex, RSINetworkOutputAddressType.RSINetworkOutputAddressTypeOVERRIDE_VALUE);
 
    // enable the network override to copy the MathBlock output to the PDO
    // only needs to be set once, probably in initialization
    controller.NetworkOutputOverrideSet((int)targetTorqueIndex, true);
 
    // MathBlock config & set (mb.value = axis0.torqueActual + BIAS)
    var mbConfig = controller.MathBlockConfigGet(MB_INDEX);
    mbConfig.InputAddress0   = torqueActualAddress;  // input0: axis 0 actual torque
    mbConfig.InputAddress1   = userBufferAddr0;      // input1: BIAS from user buffer
    mbConfig.InputDataType0  = INT32;
    mbConfig.InputDataType1  = INT32;
    mbConfig.Operation       = ADD;                  // switch to MULTIPLY for proportional bias
    mbConfig.ProcessDataType = INT32;
    mbConfig.OutputAddress   = targetTorqueAddress;  // output: axis 1 target torque override
    mbConfig.OutputDataType  = INT32;
 
    // set MathBlock and wait 1 sample for config to take effect
    controller.MathBlockConfigSet(MB_INDEX, mbConfig);
    controller.SampleWait(1);
 
    // print results
    Console.WriteLine($"MathBlock configured to copy axis {Constants.AXIS_0_INDEX} torque actual to axis {Constants.AXIS_1_INDEX} target torque with bias {BIAS}");
    Console.WriteLine($"Operation: {mbConfig.Operation}");
    Console.WriteLine("✅ MathBlock PDO copy with bias configured successfully");
}
// handle errors as needed
finally
{
    controller.Delete();  // dispose
}