PVTTriggerIO.cs

/*  
 Copyright(c) 1998-2009 by Robotic Systems Integration, Inc. All rights reserved.
 This software contains proprietary and confidential information of Robotic 
 Systems Integration, Inc. (RSI) and its suppliers. Except as may be set forth 
 in the license agreement under which this software is supplied, disclosure, 
 reproduction, or use with controls other than those provided by RSI or suppliers
 for RSI is strictly prohibited without the prior express written consent of 
 Robotic Systems Integration.

 Warning!  This is a sample program to assist in the integration of your motion 
 controller with your application.  It may not contain all of the logic and safety
 features that your application requires.
  
 This sample applications demonstrates how to trigger IO events during PVT motion. 
 This is usful for users that want to read and write inputs and outputs based on 
 motionID.

 For any questions regarding this sample code please visit www.roboticsys.com.
 ==================================================================================
*/


using System;
using System.Collections.Generic;
using System.Text;
using NUnit.Framework;
using RSI.RapidCode.SynqNet.dotNET;
using RSI.RapidCode.SynqNet.dotNET.Enums;

namespace SampleApplications
{
    [TestFixture]
    class PVTTriggerIO
    {
        const int AXIS_X = 0;
        const int AXIS_Y = 1;
        const int CONTROLLER = 0;
        const int POINTS = 3;
        const int AXES = 2;
        const int IO_NODE = 1;
        const int NUM_MOVES = 3;

        [Test]
        public void Main()
        {
            try
            {
                //Create RapidCode objects
                MotionController controller;
                Axis axisX;
                Axis axisY;
                MultiAxis multiAxis;
                IO sliceio;

                double[] pos = new double[POINTS * AXES];
                double[] t = new double[POINTS];
                double[] v = new double[POINTS * AXES];

                double[] pos1 = new double[POINTS * AXES];
                double[] t1 = new double[POINTS];
                double[] v1 = new double[POINTS * AXES];

                double[] pos2 = new double[POINTS * AXES];
                double[] t2 = new double[POINTS];
                double[] v2 = new double[POINTS * AXES];

                //Initialize controller object
                controller = MotionController.CreateFromBoard(CONTROLLER);

                //Initialize axis objects
                axisX = controller.AxisGet(AXIS_X);
                axisY = controller.AxisGet(AXIS_Y);

                //Initialize Multiaxis Objects
                multiAxis = controller.MultiAxisGet(axisX);
                multiAxis.AxisAdd(axisY);

                //Initialize Slice IO object
                sliceio = controller.IOGet(IO_NODE);

                //Set Position to 0
                axisX.PositionSet(0);
                axisY.PositionSet(0);

                //Clear faults and Enable MultiAxis group
                multiAxis.Abort();
                multiAxis.ClearFaults();
                multiAxis.AmpEnableSet(true);
                
                //Position, velocity, and time points for FIRST MovePVT
                pos[0] = 500; v[0] = 1000; t[0] = 1;
                pos[1] = 500; v[1] = 1000; t[1] = 0.5;
                pos[2] = 1000; v[2] = 1000; t[2] = 0.5;
                pos[3] = 1000; v[3] = 1000;
                pos[4] = 1500; v[4] = 1000;
                pos[5] = 1500; v[5] = 1000;
                
                //Position, velocity, and time points for SECOND MovePVT
                pos1[0] = 2000; v1[0] = 1000; t1[0] = 0.5;
                pos1[1] = 2000; v1[1] = 1000; t1[1] = 0.5;
                pos1[2] = 2500; v1[2] = 1000; t1[2] = 0.5;
                pos1[3] = 2500; v1[3] = 1000;
                pos1[4] = 3000; v1[4] = 1000;
                pos1[5] = 3000; v1[5] = 1000;

                //Position, velocity, and time points for THIRD MovePVT
                pos2[0] = 3500; v2[0] = 1000; t2[0] = 0.5;
                pos2[1] = 3500; v2[1] = 1000; t2[1] = 0.5;
                pos2[2] = 4000; v2[2] = 1000; t2[2] = 1;
                pos2[3] = 4000; v2[3] = 1000;
                pos2[4] = 4500; v2[4] = 0;
                pos2[5] = 4500; v2[5] = 0;

                
                // Hold movePVT motion until MotionHoldGate bit is set to false
                multiAxis.MotionHoldGateSet(true);
                // Define hold type is set to GATE
                multiAxis.MotionHoldTypeSet(RSIMotionAttrHoldType.RSIMotionAttrHoldTypeGATE);  
                // Turn on the hold motion attribute mask for the next motion command 
                multiAxis.MotionAttributeMaskOnSet(RSIMotionAttrMask.RSIMotionAttrMaskHOLD);

                // Load FIRST segment of PVT motion. 
                // Assign this segment a motion ID
                multiAxis.MotionIdSet(1000);
                multiAxis.MovePVT(pos, v, t, POINTS, -1, false, false);
                
                // After FIRST movePVT is declared, clear the hold motion attribute mask
                multiAxis.MotionAttributeMaskOffSet(RSIMotionAttrMask.RSIMotionAttrMaskHOLD);

                // Load SECOND segment of PVT motion. This wont execute until first MovePVT is complete.
                // Assign this segment a motion ID
                multiAxis.MotionIdSet(1001);
                multiAxis.MovePVT(pos1, v1, t1, POINTS, -1, false, false);
                                
                // Load THIRD segment of PVT motion. 
                // Assign this segment a motion ID
                multiAxis.MotionIdSet(1002);
                multiAxis.MovePVT(pos2, v2, t2, POINTS, -1, false, true);

                //Allow Motion to execute
                multiAxis.MotionHoldGateSet(false);
                Console.WriteLine("PVT Motion Begins");
                
                int OldMotionID = 0;
                int i;
                for(i= 0; i<NUM_MOVES; i++)
                {
                    //Wait here until motion ID changes
                    while (OldMotionID == multiAxis.MotionIdExecutingGet()) { }; 

                    Console.WriteLine("MotionID{0} = {1}", i, multiAxis.MotionIdExecutingGet());
                    switch (multiAxis.MotionIdExecutingGet())
                    {
                        case 1000:
                            sliceio.SegmentDigitalOutSet(0, 2, true);
                            Console.WriteLine("Digital Out High");
                            OldMotionID = 1000;
                            break;
                        case 1001:
                            sliceio.SegmentDigitalOutSet(0, 2, false);
                            Console.WriteLine("Digital Out Low");
                            OldMotionID = 1001;
                            break;
                        case 1002:
                            sliceio.SegmentDigitalOutSet(0, 2, true);
                            Console.WriteLine("Digital Out High");
                            OldMotionID = 1002;
                            break;
                        default:
                            sliceio.SegmentDigitalOutSet(0, 2, false);
                            Console.WriteLine("Digital Out Low");
                            break;
                    }    
                }

                //While loop to wait for completion of PVT motion
                multiAxis.MotionDoneWait();
                Console.WriteLine("Motion done: Disable output");
                sliceio.SegmentDigitalOutSet(0, 2, false);
                
                Console.WriteLine("PVT Motion Complete");
            }

            catch (RsiError err)
            {
                Console.WriteLine(err.text);
            }
        }
    }
}