MathBlock.cs

 
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.
    }
  }
}