MultiAxisMotion.cs

 
using RSI.RapidCode.dotNET;         // Import our RapidCode Library.
using NUnit.Framework;
using System;
 
#if DOXYGEN // RSI internal documentation use only
using RSI.RapidCode;
#endif
 
[TestFixture]
[Category("Software")]
class MultiAxisMotion : StaticMemoryTestBase
  {
    [Test]
    public void MultiAxisVelocityMotion()
    {
      // RapidCode Objects
      MultiAxis multi;      // Declare what multi is.
 
      // Constants                                
      const int cycles = 2; // Specify how many times you want to update the velocity.
      const int NUM_OF_AXES = 6;
 
      controller.AxisCountSet(NUM_OF_AXES);
      controller.MotionCountSet(NUM_OF_AXES + 1);                                                             // We will need a motion supervisor for every Axis object and MultiAxis object
 
      int i;
      double[] accelerations = new double[6] { 1000, 1000, 1000, 1000, 1000, 1000 }; // Specify the acceleration for all 6 axes.
      double[] velocities    = new double[6];                                        // Initialize the array that will contain the velocities of all 6 axes.
      Random rnd             = new Random();                                         // Initialize the Random object that we will use to generate random velocities.
 
      Axis x_axis = controller.AxisGet(Constants.X_AXIS_NUMBER); // Initialize axis. (You can use RapidSetup to see what axes you are using.)
      Axis y_axis = controller.AxisGet(Constants.Y_AXIS_NUMBER); // Initialize axis.
      Axis z_axis = controller.AxisGet(Constants.Z_AXIS_NUMBER); // Initialize axis.
      Axis a_axis = controller.AxisGet(Constants.A_AXIS_NUMBER);  // Initialize axis.
      Axis b_axis = controller.AxisGet(Constants.B_AXIS_NUMBER);  // Initialize axis.
      Axis c_axis = controller.AxisGet(Constants.C_AXIS_NUMBER);  // Initialize axis.
 
      HelperFunctions.CheckErrors(x_axis);           // Check that the axis has been initialize correctly.
      HelperFunctions.CheckErrors(y_axis);           // Check that the axis has been initialize correctly.
      HelperFunctions.CheckErrors(z_axis);           // Check that the axis has been initialize correctly.
      HelperFunctions.CheckErrors(a_axis);           // Check that the axis has been initialize correctly.
      HelperFunctions.CheckErrors(b_axis);           // Check that the axis has been initialize correctly.
      HelperFunctions.CheckErrors(c_axis);           // Check that the axis has been initialize correctly.
 
      multi = controller.MultiAxisGet(NUM_OF_AXES);               // Configure your MultiAxis on RapidSetup (Make sure MotionCount is 1 higher than AxisCount)
      HelperFunctions.CheckErrors(multi);            // Check that multi has been initialized correctly.
 
      multi.AxisRemoveAll();                                      // If there are any current axes on multi, remove them.
 
      multi.AxisAdd(x_axis);                                      // Add axis to your Multi-Axis controller.
      multi.AxisAdd(y_axis);                                      // Add axis to your Multi-Axis controller.
      multi.AxisAdd(z_axis);                                      // Add axis to your Multi-Axis controller.
      multi.AxisAdd(a_axis);                                      // Add axis to your Multi-Axis controller.
      multi.AxisAdd(b_axis);                                      // Add axis to your Multi-Axis controller.
      multi.AxisAdd(c_axis);                                      // Add axis to your Multi-Axis controller.
 
      multi.Abort();                                              // If there is any motion happening, abort it.
      multi.ClearFaults();                                        // Clear faults and enable all axes.
      multi.AmpEnableSet(true);                                   // Enable the motor.
 
      for (i = 0; i < cycles; i++)                                // This loop will iterate 5 times based on the value of "cycles"
      {
        int random_vel1 = rnd.Next(1, 100);                       // random_vel1 is a number [ >= 1 and < 100 ]
        int random_vel2 = rnd.Next(1, 100);
        int random_vel3 = rnd.Next(1, 100);
        int random_vel4 = rnd.Next(1, 100);
        int random_vel5 = rnd.Next(1, 100);
        int random_vel6 = rnd.Next(1, 100);
 
        velocities = new double[6] {random_vel1,                  // Update axis's velocity.
                                    random_vel2,                  // Update axis's velocity.
                                    random_vel3,                  // Update axis's velocity.
                                    random_vel4,                  // Update axis's velocity.
                                    random_vel5,                  // Update axis's velocity.
                                    random_vel6 };                // Update axis's velocity.
 
        multi.MoveVelocity(velocities, accelerations);            // Move your Multi-Axis. (this will also update the move on the fly)
 
        System.Threading.Thread.Sleep(100);                       // Sleep for 100ms before iterating again.
      }
 
      multi.Abort();                                              // Stop motion on all axes.
    }
 
    [Test]
    public void PointToPointMultiAxisMotion()
    {
      // Constants
      const int NUM_OF_AXES = 2; // Specify the number of axes (Make sure your axis count in RapidSetup is 2!)
 
      // Parameters
      double[] positions1    = new double[NUM_OF_AXES] { 5, 10 };       // The first set of positions to be moved to
      double[] positions2    = new double[NUM_OF_AXES] { 15, 15 };      // The second set of positions to be moved to
      double[] velocities1   = new double[NUM_OF_AXES] { 1000, 1000 };  // The velocity for the two axes for the first move- Units: units/sec (driver will execute 10 rotations per second)
      double[] velocities2   = new double[NUM_OF_AXES] { 1000, 1000 };  // The velocity for the two axes for the second move
      double[] accelerations = new double[NUM_OF_AXES] { 500, 500 };    // The acceleration for the two axes
      double[] decelerations = new double[NUM_OF_AXES] { 500, 500 };    // The deceleration for the two axes
      double[] jerkPercent   = new double[NUM_OF_AXES] { 50, 50 };      // The jerk percent for the two axes
 
      controller.AxisCountSet(NUM_OF_AXES);
      controller.MotionCountSet(NUM_OF_AXES + 1);
 
      Axis axis0 = controller.AxisGet(Constants.X_AXIS_NUMBER);         // Initialize axis0
      Axis axis1 = controller.AxisGet(Constants.Y_AXIS_NUMBER);         // Initialize axis1
 
      HelperFunctions.CheckErrors(axis0);                  // [Helper Function] Check that 'axis0' has been initialized correctly
      HelperFunctions.CheckErrors(axis1);                  // [Helper Function] Check that 'axis1' has been initialized correctly
 
      // In this application, we have two Axis objects and one MultiAxis object, so three motion supervisors are required
      MultiAxis multi = controller.MultiAxisGet(NUM_OF_AXES);           // Initialize a new MultiAxis object. MultiAxisGet takes a motion supervisor number as its argument.
                                                                        // This number is equal to the number of axes since motion supervisors are zero indexed (i.e., motion supervisors
                                                                        // 0 and 1 are used for axis0 and axis1, so motion supervisor 2 is available for our MultiAxis object).
 
      HelperFunctions.CheckErrors(multi);                  // [Helper Function] Check that 'multi' has been initialized correctly
      controller.AxisCountSet(NUM_OF_AXES);                             // Set the number of axis being used. A phantom axis will be created if for any axis not on the network. You may need to refresh rapid setup to see the phantom axis.
 
      multi.AxisRemoveAll();                                            // Remove all current axis if any. So we can add new ones
 
      multi.AxisAdd(axis0);                                             // Add axis0 to the MultiAxis object
      multi.AxisAdd(axis1);                                             // Add axis1 to the MultiAxis object
 
      multi.Abort();                                                    // If there is any motion happening, abort it
      multi.ClearFaults();                                              // Clear any faults
      multi.AmpEnableSet(true);                                         // Enable the motor
 
      axis0.ErrorLimitActionSet(RSIAction.RSIActionNONE);               // Disable poistion error for Phantom Axes.
      axis1.ErrorLimitActionSet(RSIAction.RSIActionNONE);               // Disable poistion error for Phantom Axes.
 
      multi.MoveSCurve(positions1, velocities1, accelerations, decelerations, jerkPercent); // Move to the positions specified in positions1 using a trapezoidal motion profile
      multi.MotionDoneWait();                                                               // Wait for motion to finish
 
      Assert.AreEqual(positions1[0], axis0.CommandPositionGet(), "The first axis in the multi axis object should be commanded to move to the firt element of the array");
      Assert.AreEqual(positions1[1], axis1.CommandPositionGet(), "The second axis in the multi axis object should be commanded to move to the second element of the array");
 
      multi.MoveTrapezoidal(positions2, velocities2, accelerations, decelerations);         // Move to the positions specified in positions2 using a SCurve motion profile
      multi.MotionDoneWait();                                                               // Wait for the motion to finish
 
      Assert.AreEqual(positions2[0], axis0.CommandPositionGet(), "The first axis in the multi axis object should be commanded to move to the firt element of the array");
      Assert.AreEqual(positions2[1], axis1.CommandPositionGet(), "The second axis in the multi axis object should be commanded to move to the second element of the array");
 
      multi.AmpEnableSet(false);                                                            // Disable the axes
 
      // /@[PointToPointMultiAxisMotion]
    }
  }