mathblock-calculate-acceleration-from-velocity.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 MathBlocks to calculate acceleration from velocity.
   It uses one MathBlock to subtract the previous velocity from the current velocity to get the velocity delta,
   and a second MathBlock to store the current velocity as the previous velocity for the next calculation.
   The calculated acceleration is then derived from the velocity delta and sample rate.
*/
 
using RSI.RapidCode; // RSI.RapidCode.dotNET;
 
Console.WriteLine("📜 MathBlock: Calculate Acceleration from Velocity");
 
int exitCode = 0;
 
// set sample config params
const int MB_COUNT = 2;
const int MB1 = 0;            // mathblock 1 index: subtract current velocity - previous velocity
const int MB2 = 1;            // mathblock 2 index: store current velocity as previous velocity
const double VELOCITY = 1.0;
const double ACCELERATION = 0.0123;
const RSIDataType DOUBLE = RSIDataType.RSIDataTypeDOUBLE;
const RSIMathBlockOperation SUBTRACT = RSIMathBlockOperation.RSIMathBlockOperationSUBTRACT;
const RSIMathBlockOperation MULTIPLY = RSIMathBlockOperation.RSIMathBlockOperationMULTIPLY;
 
// get RMP controller
MotionController controller = MotionController.Get();
    
try
{
    Helpers.CheckErrors(controller);
 
    Helpers.VerifyHardwareUsage(controller);
    Helpers.VerifyAxisCount(controller);
 
    // set mathblock count before any RapidCodeObject get/create other than the controller
    controller.MathBlockCountSet(MB_COUNT);
 
    // get & configure axis 
    Axis axis = controller.AxisGet(Constants.AXIS_0_INDEX);
    Helpers.CheckErrors(axis);
    if (!Constants.USE_HARDWARE) Helpers.PhantomAxisReset(axis);
    axis.PositionSet(0);
 
    // set sample addresses
    ulong userBuffer0Addr = controller.AddressGet(RSIControllerAddressType.RSIControllerAddressTypeUSER_BUFFER, 0);
    ulong previousVelocityAddr = controller.AddressGet(RSIControllerAddressType.RSIControllerAddressTypeMATHBLOCK_PROCESS_VALUE, MB2);
    ulong currentVelocityAddr = axis.AddressGet(RSIAxisAddressType.RSIAxisAddressTypeCOMMAND_VELOCITY);
 
    // write 1 to user buffer index 0 for MB2 multiplication
    controller.MemoryDoubleSet(userBuffer0Addr, 1);
 
    // MathBlock 1 config: subtract current velocity - previous velocity
    var mbConfig1 = controller.MathBlockConfigGet(MB1);
    mbConfig1.Operation       = SUBTRACT;
    mbConfig1.InputAddress0   = currentVelocityAddr;   // input0: current command velocity
    mbConfig1.InputAddress1   = previousVelocityAddr;  // input1: previous velocity (from 2nd MathBlock)  
    mbConfig1.InputDataType0  = DOUBLE;
    mbConfig1.InputDataType1  = DOUBLE;
    mbConfig1.ProcessDataType = DOUBLE;
 
    // MathBlock 2 config: multiply current velocity by 1 to store previous velocity
    var mbConfig2 = controller.MathBlockConfigGet(MB2);
    mbConfig2.Operation       = MULTIPLY;
    mbConfig2.InputAddress0   = currentVelocityAddr;  // input0: current command velocity 
    mbConfig2.InputAddress1   = userBuffer0Addr;      // input1: constant "1" from user buffer
    mbConfig2.InputDataType0  = DOUBLE;
    mbConfig2.InputDataType1  = DOUBLE;
    mbConfig2.ProcessDataType = DOUBLE;
 
    // set MathBlock configurations
    controller.MathBlockConfigSet(MB1, mbConfig1);
    controller.MathBlockConfigSet(MB2, mbConfig2);
 
    // wait 1 sample for MathBlock config to take effect
    controller.SampleWait(1);
 
    // move axis with small acceleration to verify MathBlock calculation
    axis.AmpEnableSet(true, Constants.AMP_ENABLE_MS);
    axis.MoveVelocity(VELOCITY, ACCELERATION);
    controller.SampleWait(10);
 
    // get latest sample velocity delta from MathBlock 1
    double latestVelocityDelta = controller.MathBlockProcessValueGet(MB1).Double;
 
    // reduce velocity back to 0
    axis.MoveVelocity(0, ACCELERATION);
    axis.MotionDoneWait();
 
    // get results
    double sampleRate = controller.SampleRateGet();
    double userUnits = axis.UserUnitsGet();
    double calculatedAcceleration = latestVelocityDelta * Math.Pow(sampleRate, 2) / userUnits;
 
    // rounding to 8 decimals matches encoder resolution for most systems.
    // this is more precise than typical hardware needs.
    double calculatedAccelerationRounded = Math.Round(calculatedAcceleration, 8);
 
    // print results
    Console.WriteLine($"Acceleration: {calculatedAccelerationRounded} (expected: {ACCELERATION})");
 
    // verify accuracy
    if (ACCELERATION != calculatedAccelerationRounded)
        throw new Exception("❌ MathBlock result is outside accepted tolerance.");
 
    // cleanup
    Helpers.AbortMotionObject(axis);
 
    exitCode = Constants.EXIT_SUCCESS;
}
// handle errors as needed
catch (Exception e)
{
    Console.WriteLine($"❌ Error: {e.Message}");
    exitCode = Constants.EXIT_FAILURE;
}
finally
{
    controller.Delete();  // dispose
}
 
return exitCode;