MathBlock.cs

Attention

See the following Concept pages for a detailed explanation of this sample: Math Blocks.

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.

 
using RSI.RapidCode.dotNET; // the RapidCode.NET library.
using System;
 
#if DOXYGEN // RSI internal documentation use only
using RSI.RapidCode;
#endif
 
public class MathBlockDifferenceOfPositionUserLimit : SampleApp
{
  public override int Run()
  {
    /* CONSTANTS */
    // *NOTICE* The following constants must be configured before attempting to run with hardware.
 
    // Controller Object Counts
    const int MATHBLOCK_COUNT = 1;              // minimum required MathBlocks for this sample
    const int AXIS_COUNT = 2;                   // minimum required axes for this sample
    const int USER_LIMIT_COUNT = 1;             // minimum required user limits for this sample
 
    // Axis Configuration
    const int FIRST_AXIS_INDEX = 0;             // the first axis index
    const int SECOND_AXIS_INDEX = 1;            // the second axis index
    const double USER_UNITS = 1048576;          // counts per unit (the user units)
 
    // MathBlock Configuration
    const int MATHBLOCK_INDEX = 0;              // the mathblock index
 
    // User Limit Configuration
    const int USER_LIMIT_INDEX = 0;                                   // the user limit index
    const double MAX_POSITION_DIFFERENCE = 0.5 * USER_UNITS;                       // the maximum position difference before the user limit triggers
    const RSIAction USER_LIMIT_ACTION = RSIAction.RSIActionABORT;     // the action to take when the user limit is triggered
    const int USER_LIMIT_DURATION = 0;                                // the time delay before the action is executed after the User Limit has triggered
 
    // Motion Parameters
    const double RELATIVE_POSITION = 2 * MAX_POSITION_DIFFERENCE;            // the relative position to move the axes
    const double VELOCITY = 1;                                               // the velocity to move the axes
    const double ACCELERATION = 10;                                          // the acceleration of the axes movement
    const double DECELERATION = 10;                                          // the deceleration of the axes movement
    const double JERK_PCT = 0;                                               // the jerk percentage of the axes movement
 
    // To run with hardware, set the USE_HARDWARE flag to true AFTER you have configured the parameters above and taken proper safety precautions.
    USE_HARDWARE = false;
 
    // Determine which axis address type to use based on the USE_HARDWARE flag
    RSIAxisAddressType INPUT_AXIS_ADDRESS_TYPE = (USE_HARDWARE) ?
      (RSIAxisAddressType.RSIAxisAddressTypeACTUAL_POSITION) : (RSIAxisAddressType.RSIAxisAddressTypeCOMMAND_POSITION);
 
    /* SAMPLE APP BODY */
 
    // Initialize MotionController class.
    // NOTICE: Replace "rmpPath" with the path location of the RMP.rta (usually the RapidSetup folder) 
    // if project directory is different than rapid setup directory.
    MotionController controller = HelperFunctionsCS.CreateController(/*rmpPath*/);
 
    // Use a try/catch/finally to ensure that the controller is deleted when done.
    try
    {
      HelperFunctionsCS.CheckErrors(controller);                              // [Helper Function] Check that the axis has been initialize correctly. 
 
      // Setup the controller for the appropriate hardware configuration.
      if (USE_HARDWARE)
      {
        HelperFunctionsCS.SetupControllerForHardware(controller);
      }
      else
      {
        HelperFunctionsCS.SetupControllerForPhantoms(controller, AXIS_COUNT, new int[] { FIRST_AXIS_INDEX, SECOND_AXIS_INDEX });
      }
 
      // configure the controller object counts
      controller.MathBlockCountSet(MATHBLOCK_COUNT);
      controller.MotionCountSet(AXIS_COUNT + 1);
      controller.UserLimitCountSet(USER_LIMIT_COUNT);
 
      // get both axis objects and check for errors
      Axis axis0 = controller.AxisGet(FIRST_AXIS_INDEX);
      HelperFunctionsCS.CheckErrors(axis0);
      axis0.UserUnitsSet(USER_UNITS);                                   // set the user units (counts per unit)
      axis0.PositionSet(0);                                             // set the initial position to 0
      axis0.ErrorLimitActionSet(RSIAction.RSIActionNONE);               // Set Error Limit Action.
 
      Axis axis1 = controller.AxisGet(SECOND_AXIS_INDEX);
      HelperFunctionsCS.CheckErrors(axis1);
      axis1.UserUnitsSet(USER_UNITS);                                   // set the user units (counts per unit)
      axis1.PositionSet(0);                                             // set the initial position to 0
      axis1.ErrorLimitActionSet(RSIAction.RSIActionNONE);               // Set Error Limit Action.
 
      // configure a multiaxis for the two axes
      MultiAxis multiAxis = controller.MultiAxisGet(AXIS_COUNT);
      HelperFunctionsCS.CheckErrors(multiAxis);
 
      multiAxis.AxisRemoveAll();
      multiAxis.AxisAdd(axis0);
      multiAxis.AxisAdd(axis1);
 
      multiAxis.Abort();          // make sure the multiaxis is not moving
      multiAxis.ClearFaults();    // clear any faults
 
      // read the configuration of the MathBlock
      MotionController.MathBlockConfig mathBlockConfig = controller.MathBlockConfigGet(MATHBLOCK_INDEX);
 
      // configure the MathBlock to subtract the position of the second axis from the position of the first axis
      mathBlockConfig.InputAddress0 = axis0.AddressGet(INPUT_AXIS_ADDRESS_TYPE);
      mathBlockConfig.InputDataType0 = RSIDataType.RSIDataTypeDOUBLE;
      mathBlockConfig.InputAddress1 = axis1.AddressGet(INPUT_AXIS_ADDRESS_TYPE);
      mathBlockConfig.InputDataType1 = RSIDataType.RSIDataTypeDOUBLE;
      mathBlockConfig.ProcessDataType = RSIDataType.RSIDataTypeDOUBLE;
      mathBlockConfig.Operation = RSIMathBlockOperation.RSIMathBlockOperationSUBTRACT;
 
      // set the MathBlock configuration
      controller.MathBlockConfigSet(MATHBLOCK_INDEX, mathBlockConfig);
 
      // wait a sample so we know the RMP is now processing the newly configured MathBlocks
      controller.SampleWait(1);
      Console.WriteLine("MathBlock configured to subtract the position of the second axis from the position of the first axis.");
 
      // get the address of the MathBlock's ProcessValue to use in the UserLimit
      ulong mathBlockProcessValueAddress = controller.AddressGet(RSIControllerAddressType.RSIControllerAddressTypeMATHBLOCK_PROCESS_VALUE, MATHBLOCK_INDEX);
 
      // configure the UserLimit to trigger when the absolute position difference is greater than MAX_POSITION_DIFFERENCE
      controller.UserLimitConditionSet(USER_LIMIT_INDEX, 0, RSIUserLimitLogic.RSIUserLimitLogicABS_GT, mathBlockProcessValueAddress, MAX_POSITION_DIFFERENCE);
 
      // set the UserLimit action to abort motion (Note: since the axes are in a multiaxis, the other axis will also be aborted)
      controller.UserLimitConfigSet(USER_LIMIT_INDEX, RSIUserLimitTriggerType.RSIUserLimitTriggerTypeSINGLE_CONDITION, USER_LIMIT_ACTION, FIRST_AXIS_INDEX, USER_LIMIT_DURATION);
      Console.WriteLine("UserLimit configured to trigger when the absolute position difference is greater than " + MAX_POSITION_DIFFERENCE + " and abort motion.");
 
      // command motion to trigger the UserLimit
      Console.WriteLine("Moving the axes to trigger the UserLimit...");
      axis0.AmpEnableSet(true);                                   // Enable the motor.
      axis0.MoveRelative(RELATIVE_POSITION, VELOCITY, ACCELERATION, DECELERATION, JERK_PCT);  // Move the axis to trigger the UserLimit.
      axis0.MotionDoneWait();                                     // Wait for the axis to finish moving.
 
      // disable the motor and the UserLimit
      axis0.AmpEnableSet(false);                      // Disable the motor.
      controller.UserLimitDisable(USER_LIMIT_INDEX);  // Disable User Limit.
 
      // the motion should have been aborted and both axes should be in an error state
      if (axis0.StateGet().Equals(RSIState.RSIStateERROR) &&
          axis1.StateGet().Equals(RSIState.RSIStateERROR))
      {
        Console.WriteLine("Both axes are in the error state after the UserLimit triggered (This is the intended behavior).");
        return 0;
      }
      else
      {
        Console.WriteLine("Error: The axes should be in an error state after the UserLimit triggers, but they are not.");
        Console.WriteLine("First Axis State: " + axis0.StateGet());
        Console.WriteLine("Second Axis State: " + axis1.StateGet());
        return -1;
      }
    }
    catch (Exception e)
    {
      Console.WriteLine("Error: " + e.Message);
      return -1;
    }
    finally
    {
      controller.Delete();                            // Delete the controller object.
    }
  }
}
 
public class MathBlockCalculateAccelerationFromVelocity : SampleApp
{
  public override int Run()
  {
    /* CONSTANTS */
    // *NOTICE* The following constants must be configured before attempting to run with hardware.
 
    // Controller Object Counts
    const int MATHBLOCK_COUNT = 2;              // minimum required MathBlocks for this sample
    const int AXIS_COUNT = 1;                   // minimum required axes for this sample
 
    // Axis Configuration
    const int AXIS_INDEX = 0;                   // the axis index
    const double USER_UNITS = 1048576;          // counts per unit (the user units)
 
    // MathBlock Configuration
    const int SUBTRACTION_MATHBLOCK_INDEX = 0;  // the first mathblock index (for subtraction)
 
    // the second mathblock index (for multiplying CommandVelocity * 1.0)
    const int PREVIOUS_VELOCITY_MATHBLOCK_INDEX = 1; // index must be higher here than the subtraction math block index, so the subtraction data is one sample old   
    const double ONE = 1.0;                     // we'll write 1.0 to the UserBuffer so the MathBlock can use it for multiplication
    const int USERBUFFER_INDEX = 0;                  // where we'll write 1.0 so the math block can use it
 
    // Motion Parameters
    const double VELOCITY = 1.0;                 // the velocity to move the axis
    const double ACCELERATION = 0.123;               // something small so we can check the MathBlock is working
 
    // To run with hardware, set the USE_HARDWARE flag to true AFTER you have configured the parameters above and taken proper safety precautions.
    USE_HARDWARE = false;
 
    // Determine which axis address type to use based on the USE_HARDWARE flag
    RSIAxisAddressType INPUT_AXIS_ADDRESS_TYPE = RSIAxisAddressType.RSIAxisAddressTypeCOMMAND_VELOCITY;
 
    /* SAMPLE APP BODY */
 
    // Initialize MotionController class.
    // NOTICE: Replace "rmpPath" with the path location of the RMP.rta (usually the RapidSetup folder) 
    // if project directory is different than rapid setup directory.
    MotionController controller = HelperFunctionsCS.CreateController(/*rmpPath*/);
 
    // Use a try/catch/finally to ensure that the controller is deleted when done.
    try
    {
      HelperFunctionsCS.CheckErrors(controller);                              // [Helper Function] Check that the axis has been initialize correctly.
 
      // Setup the controller for the appropriate hardware configuration.
      if (USE_HARDWARE)
      {
        HelperFunctionsCS.SetupControllerForHardware(controller);
      }
      else
      {
        HelperFunctionsCS.SetupControllerForPhantoms(controller, AXIS_COUNT, new int[] { AXIS_INDEX });
      }
 
      // configure the controller object counts
      controller.MathBlockCountSet(MATHBLOCK_COUNT);
 
      // write 1.0 to the UserBuffer so the MathBlock can use it for multiplication
      controller.MemoryDoubleSet(controller.AddressGet(RSIControllerAddressType.RSIControllerAddressTypeUSER_BUFFER, USERBUFFER_INDEX), ONE);
 
      // get Axis object and check for errors
      Axis axis = controller.AxisGet(AXIS_INDEX);
      HelperFunctionsCS.CheckErrors(axis);
      axis.UserUnitsSet(USER_UNITS);                                  // set the user units (counts per unit)
      axis.PositionSet(0);                                             // set the initial position to 0
      axis.ErrorLimitActionSet(RSIAction.RSIActionNONE);              // Set Error Limit Action.
 
      axis.Abort();               // make sure the axis is not moving
      axis.ClearFaults();         // clear any faults
 
      // read the configuration of both MathBlocks
      MotionController.MathBlockConfig subtractionConfig = controller.MathBlockConfigGet(SUBTRACTION_MATHBLOCK_INDEX);
      MotionController.MathBlockConfig previousVelocityConfig = controller.MathBlockConfigGet(PREVIOUS_VELOCITY_MATHBLOCK_INDEX);
 
      // configure the first MathBlock to subtract the previous velocity from the current velocity
      // current velocity:
      subtractionConfig.InputAddress0 = axis.AddressGet(INPUT_AXIS_ADDRESS_TYPE);
      subtractionConfig.InputDataType0 = RSIDataType.RSIDataTypeDOUBLE;
      // previous velocity: (as was calculated by the second MathBlock, so we use its ProcesValue)
      subtractionConfig.InputAddress1 = controller.AddressGet(RSIControllerAddressType.RSIControllerAddressTypeMATHBLOCK_PROCESS_VALUE, PREVIOUS_VELOCITY_MATHBLOCK_INDEX);
      subtractionConfig.InputDataType1 = RSIDataType.RSIDataTypeDOUBLE;
      subtractionConfig.ProcessDataType = RSIDataType.RSIDataTypeDOUBLE;
      subtractionConfig.Operation = RSIMathBlockOperation.RSIMathBlockOperationSUBTRACT;
 
      // configure the second MathBlock to multiply the current velocity by 1.0 (which we'll use for the previous sample's velocity)
      previousVelocityConfig.InputAddress0 = axis.AddressGet(INPUT_AXIS_ADDRESS_TYPE);
      previousVelocityConfig.InputDataType0 = RSIDataType.RSIDataTypeDOUBLE;
      previousVelocityConfig.InputAddress1 = controller.AddressGet(RSIControllerAddressType.RSIControllerAddressTypeUSER_BUFFER, USERBUFFER_INDEX);
      previousVelocityConfig.InputDataType1 = RSIDataType.RSIDataTypeDOUBLE;
      previousVelocityConfig.ProcessDataType = RSIDataType.RSIDataTypeDOUBLE;
      previousVelocityConfig.Operation = RSIMathBlockOperation.RSIMathBlockOperationMULTIPLY;
 
      // set the MathBlock configurations
      controller.MathBlockConfigSet(SUBTRACTION_MATHBLOCK_INDEX, subtractionConfig);
      controller.MathBlockConfigSet(PREVIOUS_VELOCITY_MATHBLOCK_INDEX, previousVelocityConfig);
 
      // wait a sample so we know the RMP is now processing the newly configured MathBlocks
      controller.SampleWait(1);
 
      // Set the axis to move with a very small acceleration so we can check the MathBlock is working
      axis.AmpEnableSet(true);                                   // Enable the motor.
      axis.MoveVelocity(VELOCITY, ACCELERATION);
 
      // wait several samples so we know the RMP is now processing the move command and accelerating
      controller.SampleWait(10);
 
      // keep in mind firmware velocity is in counts per sample squared, so we need to convert to UserUnits per second squared
      double calculatedAccelerationCountsPerSampleSquared = controller.MathBlockProcessValueGet(SUBTRACTION_MATHBLOCK_INDEX).Double;
 
      // reduce the velocity back to 0
      axis.MoveVelocity(0, ACCELERATION);
 
      axis.MotionDoneWait();      // Wait for the axis to finish moving.
      axis.AmpEnableSet(false);   // Disable the motor.
      
      // convert to UserUnits per second squared
      double calculatedAcceleration = calculatedAccelerationCountsPerSampleSquared * controller.SampleRateGet() * controller.SampleRateGet() / axis.UserUnitsGet();
      Console.WriteLine($"Calculated acceleration from MathBlock: {calculatedAcceleration}");
 
      // check that the newly calculated acceleration is as expected
      if (Math.Abs(calculatedAcceleration - ACCELERATION) <= 0.000001)
      {
        Console.WriteLine("The MathBlock is calculating the Axis' acceleration by subtracting previous velocity from current velocity.");
        return 0;
      }
      else
      {
        Console.WriteLine("Error: The MathBlock is not calculating the Axis' acceleration as expected.");
        return -1;
      }
    }
    catch (Exception e)
    {
      Console.WriteLine("Error: " + e.Message);
      return -1;
    }
    finally
    {
      controller.Delete();                            // Delete the controller object.
    }
  }
}