mathblock-difference-of-position-userlimit.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 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);
    Helpers.PhantomAxisReset(axis0);
 
    Axis axis1 = controller.AxisGet(Constants.AXIS_1_INDEX);
    Helpers.CheckErrors(axis1);
    Helpers.PhantomAxisReset(axis1);
 
    // 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.");
 
    // cleanup
    Helpers.AbortMotionObject(axis0);
    Helpers.AbortMotionObject(axis1);
}
// handle errors as needed
finally
{
    controller.UserLimitDisable(INDEX_ZERO);  // disable user limit
    controller.Delete();                      // dispose
}