compensator-single-axis.cs

Note

See Motion: Compensator 📜 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 demonstrates how to configure a compensator that modifies the motion of itself.
    The axis command position is used as the input to apply compensation to the same axis,
    useful for correcting mechanical non-linearities or backlash.
*/
 
using RSI.RapidCode; // RSI.RapidCode.dotNET;
 
Console.WriteLine("📜 Compensator: Single Axis");
 
int exitCode = 0;
 
// set sample config params
const int INDEX_ZERO = 0;
const int MIN = 10;
const int MAX = 110;
const int DELTA = 10;
const int POINTS = ((MAX - MIN) / DELTA) + 1;  // 11 points
 
const double VELOCITY = 20.0;
const double ACCELERATION = 30.0;
const double DECELERATION = 30.0;
const double JERK = 50.0;
 
// compensator table (values are in raw counts not user units)
//                       at position: 10 20  30  40  50 ...           ... 110
//                         adjust by: ↓  ↓   ↓   ↓   ↓  ...           ... ↓
double[] table = new double[POINTS] { 0, 2, -3, -5, -3, 2, -3, 0, 2, -3, -5 };
 
// print compensator info
Console.WriteLine($"Range: {MIN} to {MAX}, Delta: {DELTA}");
Console.WriteLine($"Points: {POINTS}");
Console.WriteLine("Compensator Table:");
for (int i = 0; i < POINTS; i++)
{
    int inputPos = MIN + (i * DELTA);
    Console.WriteLine($"@ Pos: {inputPos}, Compensate by: {table[i]}");
}
 
// get rmp controller
MotionController controller = MotionController.Get();
 
try
{
    Helpers.CheckErrors(controller);
 
    Helpers.VerifyHardwareUsage(controller);
    Helpers.VerifyAxisCount(controller);
 
    // set compensator count before any RapidCodeObject get/create other than the controller
    controller.CompensatorCountSet(1);
    controller.CompensatorPointCountSet(compensatorNumber: INDEX_ZERO, pointCount: table.Length);  
 
    // get & configure axis
    Axis axis = controller.AxisGet(axisNumber: Constants.AXIS_0_INDEX);
    Helpers.CheckErrors(axis);
    if (!Constants.USE_HARDWARE) Helpers.PhantomAxisReset(axis);
    double originalUserUnits = axis.UserUnitsGet();
    axis.UserUnitsSet(1); // For demonstration simplicity, set user units to 1 even for hardware axes, since compensators work in raw counts
 
    // verify the three values are equal before configuring compensator
    if (controller.CompensatorPointCountGet(INDEX_ZERO) == table.Length && POINTS == table.Length)
    {
        // configure compensator using AXIS_COMMAND_POSITION as input to modify same axis
        controller.CompensatorConfigSet(
            compensatorNumber: INDEX_ZERO,
            inputAxis:         axis,
            inputAxisType:     RSIAxisMasterType.RSIAxisMasterTypeAXIS_COMMAND_POSITION,
            inputAxisMinimum:  MIN,
            inputAxisMaximum:  MAX,
            inputAxisDelta:    DELTA,
            outputAxis:        axis,
            outputType:        RSICompensatorOutputType.RSICompensatorOutputTypeSINGLE,
            table:             table);
 
        // command motion
        axis.AmpEnableSet(true, Constants.AMP_ENABLE_MS);
        axis.PositionSet(MIN);
        axis.MoveSCurve(position: MIN + DELTA, VELOCITY, ACCELERATION, DECELERATION, JERK);  // move to 2nd point
        axis.MotionDoneWait();
 
        // get results
        double compPos = axis.CompensationPositionGet();
 
        // print results
        Console.WriteLine($"Compensation position: {compPos} (expected: {table[1]})");
    
        // cleanup
        axis.AmpEnableSet(false);
        axis.UserUnitsSet(originalUserUnits);
        Helpers.AbortMotionObject(axis);
 
        // verify compensation
        if (compPos != table[1])
            throw new Exception("❌ Compensator compensation position does not match expected value.");
    }
 
    exitCode = Constants.EXIT_SUCCESS;
}
// handle errors as needed
catch (Exception e)
{
    Console.WriteLine($"❌ Error: {e.Message}");
    exitCode = Constants.EXIT_FAILURE;
}
finally
{
    controller.CompensatorCountSet(0);  // restore
    controller.Delete();                // dispose
}
 
return exitCode;