GcodeMotion.cs

 
using RSI.RapidCode.dotNET; // Import our RapidCode Library.
using NUnit.Framework;
using System;
using System.Linq;
using System.Threading;
using System.IO;
using System.Runtime.InteropServices;
 
#if DOXYGEN // RSI internal documentation use only
using RSI.RapidCode;
using RSI.RapidCode.Cartesian; 
#endif
 
public class SampleGcodeCallback : GcodeCallback
{
  public override void Execute(GcodeCallbackData data)
  {
    Console.WriteLine("G-Code Callback executed: " + data.LineNumber + " " + data.LineText);
 
    // if you want to nofity the Gcode object that there's an error processing, set its error details:
    // data.UserError.number = RSIErrorMessage.RSI_ERROR_MESSAGE_DYNAMIC;
    // data.UserError.text = "This is an error from the callback.";
  }
}
 
[TestFixture]
[Category("Software")]
public class GcodeMotion : SampleAppTestBase
{
  [SetUp]
  public void Setup()
  {
    jointsMultiAxis?.AxisRemoveAll();
  }
 
  [TearDown]
  public void Teardown()
  {
    robot?.EStop();
    robot?.MotionDoneWait();
    robot?.ClearFaults();
    Robot.RobotDelete(controller, robot);
    robot = null;
 
    jointsMultiAxis?.Abort();
    Thread.Sleep(50);
    jointsMultiAxis?.ClearFaults();
  }
 
  private void VerifyModel(KinematicModel model, string expectedName, LinearUnits expectedUnits = LinearUnits.None)
  {
    var actualUnits = model.UnitsGet();
    Assert.That(actualUnits, Is.EqualTo(expectedUnits), $"Expected model units to be '{expectedUnits}' but was '{actualUnits}' instead!");
    var actualName = model.NameGet();
    Assert.That(actualName, Is.EqualTo(expectedName), $"Expected model name to be '{expectedName}' but was '{actualName}' instead!");
  }
 
  [Test]
  public void GcodeBasic()
  {
    string gcodeProgram = "G91; Sets the programming mode to RELATIVE\n" +
                      "G64; Turns off exact stop mode(Default)\n" +
                      "G1 X1.0 Y0.0 Z0.0 A1.0 F60.0; Move on USERUNIT in positive x direction at 60in/min. Moves Free axis A to position 1.0.\n" +
                      "G3 X1 Y1 I0 J1; Counter clockwise arc with a center point of 0,1,0 and end point of 1,1,0 relative to the current position\n" +
                      "M80; Show how to use an M-code with GcodeCallback!\n";
 
    // We assume the axes have been confgured and homed properly prior this this program.
    const string xLabel = "X-Axis";
    const string yLabel = "Y-Axis";
    const string zLabel = "Z-Axis";
    const string aLabel = "A-Axis";
 
    x_axis.UserLabelSet(xLabel);
    y_axis.UserLabelSet(yLabel);
    z_axis.UserLabelSet(zLabel);
    a_axis.UserLabelSet(aLabel);
 
    // The joint index of each axis is the index within the MultiAxis object.
    // "X-Axis" has joint index 0
    // "Y-Axis" has joint index 1
    // "Z-Axis" has joint index 2
    // "A-Axis" has joint index 3
    Axis[] axes = new Axis[] { x_axis, y_axis, z_axis, a_axis };
    jointsMultiAxis.AxesAdd(axes, axes.Length);
    jointsMultiAxis.ClearFaults();
    jointsMultiAxis.AmpEnableSet(true);
 
    const string modelName = "RSI_XYZA";
    const double scaling = 1.0, offset = 0.0;
    LinearModelBuilder builder = new LinearModelBuilder(modelName);
    builder.JointAdd(new LinearJointMapping(0, CartesianAxis.X)     { ExpectedLabel = xLabel, Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(1, CartesianAxis.Y)     { ExpectedLabel = yLabel, Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(2, CartesianAxis.Z)     { ExpectedLabel = zLabel, Scaling = scaling, Offset = offset });
    builder.FreeAxisAdd(new ModelAxisMapping(3)                     { ExpectedLabel = aLabel, Scaling = scaling, Offset = offset }); // Add a free axis.
 
    // Create a Robot object with a multi axis containing all joints and the kinematic type
    robot = Robot.RobotCreate(controller, jointsMultiAxis, builder, MotionController.AxisFrameBufferSizeDefault);
    HelperFunctions.CheckErrors(robot);
    
    robot.Gcode.AccelerationRateSet(1000); // UserUnits per minute squared
    
    // The free axis index here refers to the index in the RobotPosition freeAxes array. We use index 0 here to refer to the first position in that array.
    robot.Gcode.FreeAxisLetterSet('A', 0);
 
    SampleGcodeCallback callback = new SampleGcodeCallback();
    robot.Gcode.CallbackRegister(callback); // Register a callback to be called when the Gcode encounters any M-code command
 
    try
    {
      robot.Gcode.Load(gcodeProgram); // Loads in the string and prepairs it for execution
    }
    catch (Exception e)
    {
      Console.WriteLine("Error loading G-Code: " + e.Message);
      
      // get any additional G-Code errors (will give you details of every line number that has an error in parsing or processing)
      HelperFunctions.CheckErrors(robot.Gcode);
      throw e; // exit the program
    }
    // print some details about the upcoming motion
    Console.WriteLine("G-Code Line Count: " + robot.Gcode.LineCountGet());
    Console.WriteLine("G-Code Error Log Count: " + robot.Gcode.ErrorLogCountGet());
    Console.WriteLine("G-code estimated run time: " + robot.Gcode.DurationGet() + " seconds");
 
    robot.Gcode.Run(); // Starts the motion. Calling with the false arguement for non blocking behavior
    Int64 activeLineNumber = 0;
    do
    {
      Thread.Sleep(200);
      if (activeLineNumber != robot.Gcode.ExecutingLineNumberGet()) // only write if we are on a new line
      {
        activeLineNumber = robot.Gcode.ExecutingLineNumberGet();
        Console.WriteLine("G-Code Line Number: " + activeLineNumber);
      }
    } while (robot.Gcode.IsRunning());
 
    HelperFunctions.CheckErrors(robot.Gcode); // Check for any errors that may have occured during the motion
 
    Assert.That(robot.Gcode.ErrorLogCountGet(), Is.EqualTo(0), "Gcode Error Log Count is not zero.  first error: " + robot.Gcode.ErrorLogGet().Message);
    Assert.That(robot.Gcode.LineCountGet(), Is.EqualTo(gcodeProgram.Count(c => c.Equals('\n'))));
    VerifyModel(robot.ModelGet(), modelName);
  }
 
  [Test]
  public void ChangingUnits()
  {
    const string xLabel = "X-Axis";
    const string yLabel = "Y-Axis";
    const string zLabel = "Z-Axis";
    const string aLabel = "A-Axis";
    const string bLabel = "B-Axis";
    const string cLabel = "C-Axis";
 
    x_axis.UserLabelSet(xLabel);
    y_axis.UserLabelSet(yLabel);
    z_axis.UserLabelSet(zLabel);
    a_axis.UserLabelSet(aLabel);
    b_axis.UserLabelSet(bLabel);
    c_axis.UserLabelSet(cLabel);
 
    // The joint index of each axis is the index within the MultiAxis object.
    // "X-Axis" has joint index 0
    // "Y-Axis" has joint index 1
    // "Z-Axis" has joint index 2
    // "A-Axis" has joint index 3
    // "B-Axis" has joint index 4
    // "C-Axis" has joint index 5
    Axis[] axes = new Axis[] { x_axis, y_axis, z_axis, a_axis, b_axis, c_axis };
    jointsMultiAxis.AxesAdd(axes, axes.Length);
    jointsMultiAxis.ClearFaults();
 
    const string modelName = "RSI_XYZABC_Centimeters";
    const LinearUnits units = LinearUnits.Centimeters;
    const double scaling = 1.0, offset = 0.0;
    LinearModelBuilder builder = new LinearModelBuilder(modelName);
    builder.UnitsSet(units);
    builder.JointAdd(new LinearJointMapping(0, CartesianAxis.X)     { ExpectedLabel = xLabel, Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(1, CartesianAxis.Y)     { ExpectedLabel = yLabel, Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(2, CartesianAxis.Z)     { ExpectedLabel = zLabel, Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(3, CartesianAxis.Roll)  { ExpectedLabel = aLabel, Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(4, CartesianAxis.Pitch) { ExpectedLabel = bLabel, Scaling = scaling, Offset = offset });
    builder.JointAdd(new LinearJointMapping(5, CartesianAxis.Yaw)   { ExpectedLabel = cLabel, Scaling = scaling, Offset = offset });
 
    // Create a Robot object with a multi axis containing all joints and the kinematic type
    robot = Robot.RobotCreate(controller, jointsMultiAxis, builder, MotionController.AxisFrameBufferSizeDefault);
    HelperFunctions.CheckErrors(robot);
 
    //NOTE: to use the above kinematic model you must have a gantry (linear 1:1 kinematics) and each linear axis must have its user units scaled to millimeters
 
    //This will return none. A gcode unit hasn't been established to it will use path units. If path units are not set it will use user units.
    Console.WriteLine(robot.Gcode.UnitsGet()); 
 
    robot.Gcode.AccelerationRateSet(10); //Sets the G-Code acceleration to 10 Centimeters(USER UNITS) per MINUTE sqd
    robot.Gcode.FeedRateSet(10);     //Sets the G-Code velocity to 10 Centimeters(USER UNITS) per MINUTE
 
    robot.Gcode.UnitsSet(LinearUnits.Inches);//This is the same as executing the G-Code Line G20
    Console.WriteLine(robot.Gcode.UnitsGet());//This will return Inches as you just set. 
 
 
    robot.Gcode.AccelerationRateSet(10); //Sets the G-Code acceleration to 10 Inches per MINUTE sqd
    robot.Gcode.FeedRateSet(10);     //Sets the G-Code velocity to 10 Inches per MINUTE
 
    VerifyModel(robot.ModelGet(), modelName, units);
  }
}