mathblock-difference-of-position-userlimit.cs

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