mathblock-pdo-copy.cs

 
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
}