mathblock-calculate-acceleration-from-velocity.cs

using RSI.RapidCode; // RSI.RapidCode.dotNET;
 
Console.WriteLine("📜 MathBlock: Calculate Acceleration from Velocity");
 
// 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);
 
    // 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);
    axis.ErrorLimitActionSet(RSIAction.RSIActionNONE);
    axis.Abort();
    axis.ClearFaults();
    controller.SampleWait(10);
    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);
    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();
 
    // see 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($"Calculated acceleration: {calculatedAccelerationRounded}");
    Console.WriteLine($"Expected acceleration: {ACCELERATION}");
 
    // verify accuracy
    if (ACCELERATION != calculatedAccelerationRounded)
        throw new Exception("❌ MathBlock result is outside accepted tolerance.");
}
// handle errors as needed
finally
{
    controller.Delete();  // dispose
}