Math Blocks

Sample applications demonstrating use of Math Blocks in RapidCode.

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.

📜 Math Block: Calculate Acceleration from Velocity

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();
 
    // see 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($"Calculated acceleration: {calculatedAccelerationRounded}");
    Console.WriteLine($"Expected acceleration: {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

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();
 
    // see results
    var axis0State = axis0.StateGet();
 
    // print results
    Console.WriteLine($"Axis 0 expected state: {RSIState.RSIStateERROR}");
    Console.WriteLine($"Axis 0 state: {axis0State}");
 
    // 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

 
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 is not started. Please start the network before running this sample 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
}