RapidSequencerAPI.cs

using System;
using System.IO;
using System.Threading;
using System.Threading.Tasks;
using NUnit.Framework;
using RSI.RapidCode.dotNET;
using System.Diagnostics;
using System.Linq;
 
 
  //[TestFixture(RSI.RapidSequencer.Platform.Windows, 200)]
  [TestFixture(RSI.RapidSequencer.Platform.INtime, 200)]
  [Category("Software")]
  class RapidSequencerAPIExample : SampleAppTestBase
  {
#if X64
    static string samplesDirectory = TestContext.CurrentContext.TestDirectory + @"\examples\rapidsequencer\";
#else
    static string samplesDirectory = TestContext.CurrentContext.TestDirectory + @"\..\examples\rapidsequencer\"; // x86
#endif
 
    string multipleTasksSampleFile = samplesDirectory + "MultipleTasks.sq";
 
    int GLOBAL_TEST_LOOPS;// = 200;
 
    public String _nodeName;// = "NodeA";
    public String _rmpNodeName;// = "NodeA";
    public RSI.RapidSequencer.Platform _platform;// = RapidSequencer.Platform.Windows;
 
    public String workingDir = TestContext.CurrentContext.TestDirectory;
 
    public int port;
    public ulong timeoutMs;
 
    private void ShutdownSequencers()
    {
      RSI.RapidSequencer.API.KillAll();
      //var sequencers = RSI.RapidSequencer.RapidSequencerFactory.Discover(RSI.RapidSequencer.DiscoveryType.Local_All);
      //foreach (var sequencer in sequencers)
      //{
      //  sequencer.Shutdown();
      //}
    }
 
    string runShell(string cmd, string cmdArgs)
    {
      Process cliProcess = new Process()
      {
        StartInfo = new ProcessStartInfo(cmd, cmdArgs)
        {
          UseShellExecute = false,
          RedirectStandardOutput = true
        }
      };
      cliProcess.Start();
      string cliOut = cliProcess.StandardOutput.ReadToEnd();
      cliProcess.WaitForExit();
      cliProcess.Close();
 
      return cliOut;
    }
 
    bool hasValipIP(string ifconfigOutput)
    {
      string[] invalidIPs = { "0.0.0.0", "127.0.0.1" };
      System.Text.RegularExpressions.Regex ipv4Regex = new System.Text.RegularExpressions.Regex(@"inet\s+\b(\d{1,3}\.\d{1,3}\.\d{1,3}\.\d{1,3})\b");
 
      System.Text.RegularExpressions.Match ipv4Match = ipv4Regex.Match(ifconfigOutput);
 
      while (ipv4Match.Success)
      {
        string ipv4String = ipv4Match.Groups[1].Value;
        if (!invalidIPs.Contains(ipv4String))
        {
          return true;
        }
 
        ipv4Match = ipv4Match.NextMatch();
      }
      return false;
    }
 
    void ConfigureINtimeNetworkStack()
    {
      string intime_base = Environment.ExpandEnvironmentVariables("%intime%");
      string piperta = intime_base + "bin\\piperta";
      string ifconfig = "-node " + this._nodeName + " \"" + intime_base + "network7\\ifconfig\"";
      string netload = "-node " + this._nodeName + " \"" + intime_base + "network7\\netload\"";
 
      string ifconfigOut = runShell(piperta, ifconfig);
      bool validIP = hasValipIP(ifconfigOut);
      if (validIP)
      {
        return;
      }
 
      runShell(piperta, netload);
 
      ifconfigOut = runShell(piperta, ifconfig);
 
      validIP = hasValipIP(ifconfigOut);
 
      Assert.IsTrue(validIP, "ifconfig did not return a valid IP address (even after netload)!");
    }
 
    [OneTimeSetUp]
    public void SetupFixture()
    {
      if (this._platform == RSI.RapidSequencer.Platform.INtime)
      {
        ConfigureINtimeNetworkStack();
      }
      ShutdownSequencers();
 
      if (controller != null)
      {
        controller.InterruptEnableSet(false);
        controller.Delete();
      }
      controller = MotionController.CreateFromSoftware(TestContext.CurrentContext.TestDirectory);
      int frameSendReceiveMicroseconds = 0; // no need for the firmware to wait for EtherCAT frames to send/receive since this is a software test
      controller.MemorySet(controller.AddressGet(RSIControllerAddressType.RSIControllerAddressTypeFRAME_SEND_RECEIVE_MICROSECONDS), frameSendReceiveMicroseconds);
 
      for (int index = 0; index < controller.AxisCountGet(); index++)
      {
        AxisSetup(controller.AxisGet(index));
      }
    }
 
    private void FastWaitForDone(Axis axis)
    {
      const int MAX_LOOPS = 100;
      int loops = 0;
      while (!axis.MotionDoneGet())
      {
        Thread.Sleep(1);
        loops++;
        if (loops > MAX_LOOPS)
          Assert.Fail("Failed waiting for motion done (fast) on Axis " + axis.NumberGet());
      }
    }
 
    public static UInt64 DedidcatedInputAddressGet(Axis axis)
    {
      // Be sure to thank Shota for the great~ helper method
      var dedicatedInPointerAddress = axis.AddressGet(RSIAxisAddressType.RSIAxisAddressTypeDEDICATED_INPUTS_POINTER);
      var dedicatedInPointer = axis.rsiControl.MemoryGet(dedicatedInPointerAddress);
      var dedicatedInAddress = axis.rsiControl.HostAddressGet((uint)dedicatedInPointer);
      return dedicatedInAddress;
    }
 
    public void AxisSetup(Axis axis)
    {
      while (axis.ErrorLogCountGet() > 0)
      {
        axis.ErrorLogClear();
      }
      axis.ThrowExceptions(true);
 
      const double USER_UNITS = 1000.0;
      axis.UserUnitsSet(USER_UNITS);
 
      axis.InterruptEnableSet(false);
 
      const double ACCELERATION = 1000000;
      axis.EStopDecelerationSet(ACCELERATION);
 
      axis.MotorTypeSet(RSIMotorType.RSIMotorTypePHANTOM);
      axis.AmpFaultActionSet(RSIAction.RSIActionNONE);
      axis.HardwareNegLimitActionSet(RSIAction.RSIActionNONE);
      axis.HardwareNegLimitTriggerStateSet(true); // default
      axis.HardwarePosLimitActionSet(RSIAction.RSIActionNONE);
      axis.HardwarePosLimitTriggerStateSet(true); // default
      axis.ErrorLimitActionSet(RSIAction.RSIActionNONE);
 
      const double COARSE_POSITION_PHANTOM = Double.MaxValue / 10;
      axis.PositionToleranceCoarseSet(COARSE_POSITION_PHANTOM);
 
      const double FINE_POSITION_PHANTOM = Double.MaxValue / 10;
      axis.PositionToleranceFineSet(FINE_POSITION_PHANTOM);
      ulong dedicatedInAddress = DedidcatedInputAddressGet(axis);
      axis.rsiControl.MemorySet(dedicatedInAddress, 0); // clear all dedicated inputs (this is the default/startup state for phantoms)
 
      // clear faults, enable, etc
      axis.Abort();
      FastWaitForDone(axis);
      axis.ClearFaults();
      Assert.That(axis.StateGet(), Is.EqualTo(RSIState.RSIStateIDLE));
      axis.PositionSet(0);
      axis.CommandPositionSet(0);
      axis.CommandPositionDirectSet(0);
      axis.MotionCamRepeatStop();
      axis.DefaultVelocitySet(Constants.VELOCITY);
      axis.DefaultAccelerationSet(Constants.ACCELERATION);
      axis.DefaultDecelerationSet(Constants.ACCELERATION);
      axis.DefaultJerkPercentSet(Constants.JERK_PERCENT);
      axis.BacklashWidthSet(0.0);
      axis.BacklashRateSet(0.0);
 
      //reset gearing
      axis.GearingDisable();
 
      axis.FilterAlgorithmSet(RSIFilterAlgorithm.RSIFilterAlgorithmPID); // default
 
      // restore default attr mask
      axis.MotionAttributeMaskDefaultSet();
      axis.MotionHoldTypeSet(RSIMotionHoldType.RSIMotionHoldTypeNONE);
 
      Assert.That(axis.StateGet(), Is.EqualTo(RSIState.RSIStateIDLE), "Axis  " + axis.NumberGet() + " could not clear faults in AxisSetup.   Source is: " + axis.SourceNameGet(axis.SourceGet()));
      Assert.That(axis.CommandPositionGet(), Is.EqualTo(0), "Axis  " + axis.NumberGet() + " command position is not zero.");
 
      axis.FeedRateSet(1.0);  // restore default FeedRate
 
      // restore defaults
      axis.EStopDecelerationSet(10000.0);
      axis.EStopJerkPercentSet(50.0);
      axis.TriggeredModifyDecelerationSet(10000.0);
      axis.TriggeredModifyJerkPercentSet(50.0);
      axis.SoftwareNegLimitActionSet(RSIAction.RSIActionE_STOP);
      axis.SoftwarePosLimitActionSet(RSIAction.RSIActionE_STOP);
      axis.SoftwareNegLimitTriggerValueSet(-1125899906842620);  // this is what shows as default in RapidSetup
      axis.SoftwarePosLimitTriggerValueSet(1125899906842620);  // this is what shows as default in RapidSetup
      axis.ErrorLimitTriggerValueSet(1000);
 
      axis.FilterDualLoopSet((Axis)axis, RSIMotorFeedback.RSIMotorFeedbackPRIMARY, false);  // setting to false should restore to default
 
      axis.HomeOffsetSet(0.0); // restoring this to zero shaved 11 seconds off Axis tests!!!
 
      axis.ClearFaults();
      axis.AmpEnableSet(true);
    }
 
    [OneTimeTearDown]
    public void TeardownFixture()
    {
      ShutdownSequencers();
    }
 
    [TearDown]
    public void TearDownSequencers()
    {
      ShutdownSequencers();
    }
 
    public RapidSequencerAPIExample(RSI.RapidSequencer.Platform platform, int loops)
 
    {
      this._platform = platform;
 
      this._nodeName = "NodeB";
      this._rmpNodeName = "NodeA";
 
      GLOBAL_TEST_LOOPS = loops;
 
      port = 50051;
      timeoutMs = 20000;
    }
 
    
    [Test]
    public void TwoTasksSimultaneously()
    {
      Assert.That(this._platform, Is.EqualTo(RSI.RapidSequencer.Platform.INtime), "Timing tests are only valid on real-time operating systems!");
 
      // Create two RapidSequencer instances
      RSI.RapidSequencer.RapidSequencer sequencer1 = RSI.RapidSequencer.RapidSequencerFactory.Create(this._platform, _nodeName, _rmpNodeName, workingDir, port, $"Sequencer-{port}", timeoutMs);
      RSI.RapidSequencer.RapidSequencer sequencer2 = RSI.RapidSequencer.RapidSequencerFactory.Create(this._platform, _nodeName, _rmpNodeName, workingDir, port + 1, $"Sequencer-{port + 1}", timeoutMs);
 
      // Call EngineStart() on both sequencers before performing any other actions.
      string rmpPath = TestContext.CurrentContext.TestDirectory;
      sequencer1.EngineStart(this._rmpNodeName, rmpPath);
      sequencer2.EngineStart(this._rmpNodeName, rmpPath);
 
      // Compile the same RapidScript program, but with different entry points.
      sequencer1.Compile(multipleTasksSampleFile, "RunEverySample");
      sequencer2.Compile(multipleTasksSampleFile, "RunEvery50Samples");
 
      // Start both tasks and run them asynchronously.
      string runEverySampleTaskId = sequencer1.RunAsync();
      string runEvery50SamplesTaskId = sequencer2.RunAsync();
 
      // Wait a little. These tasks run forever. Dont want to wait for them to finish.
      Thread.Sleep(1000);
 
      for (int i = 0; i < GLOBAL_TEST_LOOPS; i++)
      {
        // Get the values of the global variables.
        RSI.RapidSequencer.SequencerGlobal loopCout = sequencer1.GlobalVariableGet("loopCounterDelta");
        RSI.RapidSequencer.SequencerGlobal loopCout50 = sequencer2.GlobalVariableGet("loopCounterDelta50");
 
        // Perform some checks on the values of the global variables.
        // These asserts are just for demonstration and testing purposes, and are not be necessary in an actual application.
        Assert.That(loopCout.Type, Is.EqualTo(RSI.RapidSequencer.SequencerGlobal.DataType.Int32), "Loop counter for every sample must be int32");
        Assert.That(loopCout.Value, Is.EqualTo(1), "This must be 1 if the program is running every sample. Failed on loop " + i);
        Assert.That(loopCout50.Type, Is.EqualTo(RSI.RapidSequencer.SequencerGlobal.DataType.Int32), "Loop counter for every 50 samples must be int32");
        Assert.That(loopCout50.Value, Is.EqualTo(50), "This must be 50 if the program is running every 50 samples. Failed on loop " + i);
 
        Thread.Sleep(2); // wait a couple samples so the values are updated
      }
 
      // Force the tasks to stop
      sequencer1.TaskStop(runEverySampleTaskId);
      sequencer2.TaskStop(runEvery50SamplesTaskId);
    }
 
 
    private void RunFunctionAndTestGlobal(string functionName, string globalName, int expectedValue)
    {
      Assert.That(this._platform, Is.EqualTo(RSI.RapidSequencer.Platform.INtime), "Timing tests are only valid on real-time operating systems!");
 
      RSI.RapidSequencer.RapidSequencer sequencer = RSI.RapidSequencer.RapidSequencerFactory.Create(this._platform, _nodeName, _rmpNodeName, workingDir, port, "RapidSequencer", timeoutMs);
 
      string rmpPath = TestContext.CurrentContext.TestDirectory;
      sequencer.EngineStart(this._rmpNodeName, rmpPath);
 
      sequencer.Compile(multipleTasksSampleFile, functionName);
 
      string taskId = sequencer.RunAsync();
 
      Thread.Sleep(1000); // wait a litte for it to get running
 
      NUnit.Framework.Constraints.IResolveConstraint constraint;
      if (this._platform == RSI.RapidSequencer.Platform.INtime)
      {
        constraint = Is.EqualTo(expectedValue);
      }
      else
      {
        constraint = Is.GreaterThanOrEqualTo(expectedValue);
      }
 
      for (int i = 0; i < GLOBAL_TEST_LOOPS; i++)
      {
        RSI.RapidSequencer.SequencerGlobal globalValue = sequencer.GlobalVariableGet(globalName);
        Assert.That(globalValue.Type, Is.EqualTo(RSI.RapidSequencer.SequencerGlobal.DataType.Int32), "Global value must be int32");
        Assert.That(globalValue.Value, constraint, "This must be " + expectedValue + " if the program is running every " + expectedValue + " samples. Failed on loop #" + i);
        Thread.Sleep(1); // wait at least one sample so the values are updated
      }
 
      sequencer.TaskStop(taskId);
    }
 
    [Test]
    public void RunEverySample()
    {
      RunFunctionAndTestGlobal("RunEverySample", "loopCounterDelta", 1);
    }
 
    [Test]
    public void RunEvery50Samples()
    {
      RunFunctionAndTestGlobal("RunEvery50Samples", "loopCounterDelta50", 50);
    }
 
 
    //@[GlobalDataExample]
    [Test]
    public void BasicGlobalData()
    {
      // Create a sequencer using the factory
      // platform = the platform the RapidSequencerSequencer runs on (RSI.RapidSequencer.Platform.INtime or RSI.RapidSequencer.Platform.Windows)
      // _nodeName = the INtime node the RapidSequencer will run on
      // _rmpNodeName = the INtime node the RMP runs on
      // workingDir = the directory where the RapidSequencer executable is located
      // port = the port the RapidSequencer listens on
      // timeoutMs = the timeout in milliseconds after which Create() fails if the sequencer hasn't been created yet.
      RSI.RapidSequencer.RapidSequencer sequencer = RSI.RapidSequencer.RapidSequencerFactory.Create(_platform, _nodeName, _rmpNodeName, workingDir, port, "RapidSequencer", timeoutMs);
 
      // Start the sequencer. The sequencer must call `EngineStart()` before calling any other functions.
      // rmpPath = the path to RMP.rta
      string rmpPath = TestContext.CurrentContext.TestDirectory;
      sequencer.EngineStart(_rmpNodeName, rmpPath);
 
      // Compile the RapidSequencer script, giving it the path to the file and the name of the function to run
      sequencer.Compile(samplesDirectory + "BasicGlobalData.sq", "main");
 
      // Run the RapidSequencer asynchronously so we can adjust values while it's running.
      string taskId = sequencer.RunAsync();
 
      // Wait a second for it to run for a bit. This sleep is just for demonstration purposes, and may not be necessary in an actual application
      Thread.Sleep(1000);
 
 
      string globalIntName = "globalInt"; // the name of the global integer as a string.
      int expectedIntValue = 2; // The value that we expect the integer to have.
 
      // Get the value of a global integer in the RapidScript program, referring to it by name.
      RSI.RapidSequencer.SequencerGlobal globalInt = sequencer.GlobalVariableGet(globalIntName);
 
      // Make sure that the type is a 32-bit integer.
      if (globalInt.Type == RSI.RapidSequencer.SequencerGlobal.DataType.Int32)
      {
        int intValue = globalInt.Value;
 
        // This line does not need to be in most applications, and is just for demonstration / testing purposes.
        Assert.That(intValue, Is.EqualTo(expectedIntValue));
      }
      Assert.That(globalInt.Type, Is.EqualTo(RSI.RapidSequencer.SequencerGlobal.DataType.Int32), $"Global variable {globalIntName} was not of type int32!");
 
      // Set the value of a global double in the RapidSequencer script
      string globalDoubleName = "globalDouble"; // The name of the global double as a string.
      double expectedDoubleValue = globalInt.Value * 3.14159; // The value we want to set the double value to.
 
      // The data type of the global value we're setting. If we want to set a global double, this must be SequencerGlobal.DataType.Double
      RSI.RapidSequencer.SequencerGlobal.DataType dataType = RSI.RapidSequencer.SequencerGlobal.DataType.Double;
      
      // Set the value of the global double named "globalDouble" to the desired value.
      sequencer.GlobalVariableSet(globalDoubleName, dataType, expectedDoubleValue);
 
      // Check that the global double's value was changed to what we expected
      // Get the value of a global double in the RapidScript program, referring to it by name.
      RSI.RapidSequencer.SequencerGlobal globalDouble = sequencer.GlobalVariableGet(globalDoubleName);
 
      // Make sure that the type is a double.
      if (globalDouble.Type == RSI.RapidSequencer.SequencerGlobal.DataType.Double)
      {
        double doubleValue = globalDouble.Value;
 
        // This line does not need to be in most applications, and is just for demonstration / testing purposes.
        Assert.That(doubleValue, Is.EqualTo(expectedDoubleValue));
      }
      Assert.That(globalDouble.Type, Is.EqualTo(RSI.RapidSequencer.SequencerGlobal.DataType.Double), $"Global variable {globalDoubleName} was not of type double!");
 
      // Wait a second for the value to be updated then printed (printing is a slow operation)
      Thread.Sleep(1000);
 
      // Check the console output of the RapidSequencer
      string output = sequencer.TaskOutputGet(taskId);
      StringAssert.Contains($"{expectedDoubleValue}", output, $"Sequencer program output does not contain expected value {expectedDoubleValue}. Output: {output}");
 
      // Stop the task
      sequencer.TaskStop(taskId);
    }
    //@[GlobalDataExample]
 
  [Test]
  public void MoveSCurve()
  {
    string fileNameString = "MoveSCurve.sq";
 
    RSI.RapidSequencer.RapidSequencer sequencer = RSI.RapidSequencer.RapidSequencerFactory.Create(_platform, _nodeName, _rmpNodeName, workingDir, port, "RapidSequencer");
 
    string rmpPath = TestContext.CurrentContext.TestDirectory;
    sequencer.EngineStart(_rmpNodeName, rmpPath);
 
    string entryPoint = "main";
    sequencer.Compile(samplesDirectory + fileNameString, entryPoint);
 
    string taskId = sequencer.Run();
 
    string output = sequencer.TaskOutputGet(taskId);
 
    double expectedFinalPosition = 1.0;
    Console.WriteLine($"output: {output}");
    StringAssert.Contains($"{expectedFinalPosition}", output, $"Sequencer program axis move did not reach final destination: {expectedFinalPosition}. Output: {output}");
    
  }
}