mathblock-pdo-copy.cs

Note

See IO: Math Blocks 📜 for a detailed explanation of this sample code.

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.

/*  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);
    Helpers.PhantomAxisReset(axis0);
 
    // get & configure axis 1
    Axis axis1 = controller.AxisGet(Constants.AXIS_1_INDEX);
    Helpers.CheckErrors(axis1);
    Helpers.PhantomAxisReset(axis1);
 
    // 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
}