compensator-2d.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 and use a 2-dimensional compensator.
    A 2D compensation table is applied to a Z axis based on the positions of X and Y axes,
    useful for flatness compensation or surface mapping applications.
*/
 
using RSI.RapidCode; // RSI.RapidCode.dotNET;
 
Console.WriteLine("📜 Compensator: 2D");
 
// set sample config params
const int INDEX_ZERO = 0;
const int X_MIN = 0;
const int X_MAX = 500;
const int X_DELTA = 100;
const int X_POINTS = ((X_MAX - X_MIN) / X_DELTA) + 1;  // 6 points
const int Y_MIN = 0;
const int Y_MAX = 500;
const int Y_DELTA = 100;
const int Y_POINTS = ((Y_MAX - Y_MIN) / Y_DELTA) + 1;  // 6 points
const int TOTAL_POINTS = X_POINTS * Y_POINTS;          // 36 points total
 
// compensator table values use axis COUNTS (not user units)
// table is organized as rows (X) & columns (Y)
double[] table = new double[TOTAL_POINTS] {
    0,    0,    0,    0,    0,    0,
  100,  200, -200,   10,  300,    0,
  100,  200, -500,  400,  500,    0,
    0,    0,    0,    0,    0,    0,
 -300,  300, -300, -300, -300,    0,
    0,    0,    0,    0,    0,    0,
};
 
// print compensator info
Console.WriteLine($"X Range: {X_MIN} to {X_MAX}, Delta: {X_DELTA}");
Console.WriteLine($"Y Range: {Y_MIN} to {Y_MAX}, Delta: {Y_DELTA}");
Console.WriteLine($"Points: {TOTAL_POINTS}");
Console.WriteLine($"2D Compensator Table ({X_POINTS}x{Y_POINTS}):");
for (int i = 0; i < Y_POINTS; i++)
{
    for (int j = 0; j < X_POINTS; j++)
    {
        Console.Write($"{table[i * X_POINTS + j],6}");
    }
    Console.WriteLine();
}
 
// get rmp controller
MotionController controller = MotionController.Get();
 
try
{
    Helpers.CheckErrors(controller);
 
    // set compensator count before any RapidCodeObject get/create other than the controller
    controller.CompensatorCountSet(1);
    controller.CompensatorPointCountSet(INDEX_ZERO, table.Length);  // set number points allocated on the controller
 
    // get & configure axes
    Axis x = controller.AxisGet(axisNumber: 0);
    Helpers.CheckErrors(x);
 
    Axis y = controller.AxisGet(axisNumber: 1);
    Helpers.CheckErrors(y);
 
    Axis z = controller.AxisGet(axisNumber: 2);
    Helpers.CheckErrors(z);
 
    // set position error limit trigger
    z.ErrorLimitTriggerValueSet(1);  
 
    // init the 2D compensator
    controller.CompensatorConfigSet(
        compensatorNumber:      INDEX_ZERO,
        firstInputAxis:         x,
        firstInputAxisType:     RSIAxisMasterType.RSIAxisMasterTypeAXIS_ACTUAL_POSITION,
        firstInputAxisMinimum:  X_MIN,
        firstInputAxisMaximum:  X_MAX,
        firstInputAxisDelta:    X_DELTA,
        secondInputAxis:        y,
        secondInputAxisType:    RSIAxisMasterType.RSIAxisMasterTypeAXIS_ACTUAL_POSITION,
        secondInputAxisMinimum: Y_MIN,
        secondInputAxisMaximum: Y_MAX,
        secondInputAxisDelta:   Y_DELTA,
        outputAxis:             z,
        outputType:             RSICompensatorOutputType.RSICompensatorOutputTypeSINGLE,
        table:                  table);
 
    // get results
    x.PositionSet(0);
    y.PositionSet(0);
    double compPos1 = z.CompensationPositionGet();
    
    x.PositionSet(X_DELTA);
    y.PositionSet(Y_DELTA);
    double compPos2 = z.CompensationPositionGet();
    
    // print results
    Console.WriteLine($"Compensation at (0,0): {compPos1} (expected: {table[0]})");
    Console.WriteLine($"Compensation at ({X_DELTA},{Y_DELTA}): {compPos2} (expected: {table[7]})");
 
    // verify accuracy
    if (compPos1 != table[0] || compPos2 != table[7])
        throw new Exception("❌ Compensator compensation position does not match expected value.");
 
    // cleanup
    Helpers.AbortMotionObject(x);
    Helpers.AbortMotionObject(y);
    Helpers.AbortMotionObject(z);
}
// handle errors as needed
finally
{
    controller.CompensatorCountSet(0);  // restore
    controller.Delete();                // dispose
}