Gantry.cs

Configure a gantry which has two motors, two encoders.

/* 
 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.

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

 * 
 * This sample will configure two parallel axes (X1, X2) connected to the same load, 
 * and configure them so that X1 will be used to command linear motion, and X2 will 
 * be used to command yaw motion.
 * 
*/

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]
    public class Gantry
    {
        // RapidCode objects
        MotionController mc;
        Axis linearAxis;
        Axis yawAxis;

        // constants        
        const int X1 = 5;  // axis number
        const int X2 = 6;  // axis number
        const string mapFile = "c:\\synqnet\\stdmei.map";

        int defaultEncoderNumerator = 0;   // 0 disables
        int defaultEncoderDenominator = 0;
        int gantryEncoderNumerator = 1;   // we want 1/2 
        int gantryEncoderDenominator = 2;
        

        RSIFloatIntUnion x1PrimaryCoeff = new RSIFloatIntUnion(1.0f);
        RSIFloatIntUnion x1SecondaryCoeff = new RSIFloatIntUnion(1.0f);
        RSIFloatIntUnion x2PrimaryCoeff = new RSIFloatIntUnion(1.0f);
        RSIFloatIntUnion x2SecondaryCoeff = new RSIFloatIntUnion(-1.0f);
        RSIFloatIntUnion defaultPrimaryCoeff = new RSIFloatIntUnion(1.0f);
        RSIFloatIntUnion defaultSecondaryCoeff = new RSIFloatIntUnion(0.0f);



        // stuff we'll need
        int x1EncoderDeltaAddress;
        int x2EncoderDeltaAddress;
        int x1FilterPrimaryPointerAddress;
        int x1FilterSecondaryPointerAddress;
        int x2FilterPrimaryPointerAddress;
        int x2FilterSecondaryPointerAddress;
        int x1FilterPrimaryCoefficientAddress;
        int x1FilterSecondaryCoefficientAddress;
        int x2FilterPrimaryCoefficientAddress;
        int x2FilterSecondaryCoefficientAddress;
        int x1AxisLinkAddress;
        int x2AxisLinkAddress;
   
              

        [Test]
        public void TurnOnGantry()
        {
            GantryEnable(true);
        }


        [Test]
        public void TurnOffGantry()
        {
            GantryEnable(false);
        }


        private void GantryEnable(bool enable)
        {
            mc = MotionController.Create();
            linearAxis = mc.AxisGet(X1);
            yawAxis = mc.AxisGet(X2);
            ReadAddressesFromMotionController();

            linearAxis.Abort();
            yawAxis.Abort();

            SetupEncoderMixing(enable);
            SetupFilterMixing(enable);

            linearAxis.ClearFaults();
            linearAxis.AmpEnableSet(true);
            yawAxis.ClearFaults();
            yawAxis.AmpEnableSet(true);

        }

        private void ReadAddressesFromMotionController()
        {
            x1EncoderDeltaAddress = mc.AddressFromStringGet("Motor[" + X1.ToString() + "].IO.Encoder[0].Delta", mapFile);
            x2EncoderDeltaAddress = mc.AddressFromStringGet("Motor[" + X2.ToString() + "].IO.Encoder[0].Delta", mapFile);
            x1FilterPrimaryPointerAddress = mc.AddressFromStringGet("ControlLaw.Standard.Filter[" + X1.ToString() + "].Axis[0].Ptr", mapFile);
            x1FilterSecondaryPointerAddress = mc.AddressFromStringGet("ControlLaw.Standard.Filter[" + X1.ToString() + "].Axis[1].Ptr", mapFile);
            x2FilterPrimaryPointerAddress = mc.AddressFromStringGet("ControlLaw.Standard.Filter[" + X2.ToString() + "].Axis[0].Ptr", mapFile);
            x2FilterSecondaryPointerAddress = mc.AddressFromStringGet("ControlLaw.Standard.Filter[" + X2.ToString() + "].Axis[1].Ptr", mapFile);
            x1FilterPrimaryCoefficientAddress = mc.AddressFromStringGet("ControlLaw.Standard.Filter[" + X1.ToString() + "].Axis[0].Coeff", mapFile);
            x1FilterSecondaryCoefficientAddress = mc.AddressFromStringGet("ControlLaw.Standard.Filter[" + X1.ToString() + "].Axis[1].Coeff", mapFile);
            x2FilterPrimaryCoefficientAddress = mc.AddressFromStringGet("ControlLaw.Standard.Filter[" + X2.ToString() + "].Axis[0].Coeff", mapFile);
            x2FilterSecondaryCoefficientAddress = mc.AddressFromStringGet("ControlLaw.Standard.Filter[" + X2.ToString() + "].Axis[1].Coeff", mapFile);

            x1AxisLinkAddress = mc.AddressFromStringGet("Axis[" + X1.ToString() + "].Link", mapFile);
            x2AxisLinkAddress = mc.AddressFromStringGet("Axis[" + X2.ToString() + "].Link", mapFile);
        }


        private void SetupEncoderMixing(bool enableGantry)
        {
            if (enableGantry)
            {
                // first scale encoders by half
                linearAxis.EncoderRatioSet(RSIMotorEncoder.RSIMotorEncoderPRIMARY, gantryEncoderNumerator, gantryEncoderDenominator);
                yawAxis.EncoderRatioSet(RSIMotorEncoder.RSIMotorEncoderPRIMARY,  gantryEncoderNumerator, gantryEncoderDenominator);

                mc.OS.Sleep(10);

                // mix encoders (add on linear, subtract for yaw)
                linearAxis.FeedbackDeltaPointerSet(RSIAxisPositionInput.RSIAxisPositionInputFIRST, x1EncoderDeltaAddress);
                linearAxis.FeedbackDeltaPointerSet(RSIAxisPositionInput.RSIAxisPositionInputSECOND, x2EncoderDeltaAddress);
                linearAxis.GantryTypeSet(RSIAxisGantryType.RSIAxisGantryTypeADD);

                yawAxis.FeedbackDeltaPointerSet(RSIAxisPositionInput.RSIAxisPositionInputFIRST, x1EncoderDeltaAddress);
                yawAxis.FeedbackDeltaPointerSet(RSIAxisPositionInput.RSIAxisPositionInputSECOND, x2EncoderDeltaAddress);
                yawAxis.GantryTypeSet(RSIAxisGantryType.RSIAxisGantryTypeSUBTRACT);
            }

            else
            {
                linearAxis.EncoderRatioSet(RSIMotorEncoder.RSIMotorEncoderPRIMARY, defaultEncoderNumerator, defaultEncoderDenominator);
                yawAxis.EncoderRatioSet(RSIMotorEncoder.RSIMotorEncoderPRIMARY, defaultEncoderNumerator, defaultEncoderDenominator);

                mc.OS.Sleep(10);

                linearAxis.FeedbackDeltaPointerSet(RSIAxisPositionInput.RSIAxisPositionInputFIRST, x1EncoderDeltaAddress);
                linearAxis.FeedbackDeltaPointerSet(RSIAxisPositionInput.RSIAxisPositionInputSECOND, x1EncoderDeltaAddress);
                linearAxis.GantryTypeSet(RSIAxisGantryType.RSIAxisGantryTypeNONE);

                yawAxis.FeedbackDeltaPointerSet(RSIAxisPositionInput.RSIAxisPositionInputFIRST, x2EncoderDeltaAddress);
                yawAxis.FeedbackDeltaPointerSet(RSIAxisPositionInput.RSIAxisPositionInputSECOND, x2EncoderDeltaAddress);
                yawAxis.GantryTypeSet(RSIAxisGantryType.RSIAxisGantryTypeNONE);
            }
        }

        private void SetupFilterMixing(bool enableGantry)
        {
            if (enableGantry)
            {
                // mix X1 filter
                mc.MemorySet(x1FilterPrimaryPointerAddress, mc.FirmwareAddressGet(x1AxisLinkAddress));
                mc.MemorySet(x1FilterSecondaryPointerAddress, mc.FirmwareAddressGet(x2AxisLinkAddress));

                // mix X2 filter
                mc.MemorySet(x2FilterPrimaryPointerAddress, mc.FirmwareAddressGet(x1AxisLinkAddress));
                mc.MemorySet(x2FilterSecondaryPointerAddress, mc.FirmwareAddressGet(x2AxisLinkAddress));

                // setup X1 filter mixing coefficients
                mc.MemorySet(x1FilterPrimaryCoefficientAddress, x1PrimaryCoeff.signed32);
                mc.MemorySet(x1FilterSecondaryCoefficientAddress, x1SecondaryCoeff.signed32);

                // setup X2 filter mixing coefficients
                mc.MemorySet(x2FilterPrimaryCoefficientAddress, x2PrimaryCoeff.signed32);
                mc.MemorySet(x2FilterSecondaryCoefficientAddress, x2SecondaryCoeff.signed32);

            }

            else
            {
                // unmix X1 filter
                mc.MemorySet(x1FilterPrimaryPointerAddress, mc.FirmwareAddressGet(x1AxisLinkAddress));
                mc.MemorySet(x1FilterSecondaryPointerAddress, mc.FirmwareAddressGet(x1AxisLinkAddress));

                // unmix X2 filter
                mc.MemorySet(x2FilterPrimaryPointerAddress, mc.FirmwareAddressGet(x2AxisLinkAddress));
                mc.MemorySet(x2FilterSecondaryPointerAddress, mc.FirmwareAddressGet(x2AxisLinkAddress));

                // setup X1 filter defult coefficients
                mc.MemorySet(x1FilterPrimaryCoefficientAddress, defaultPrimaryCoeff.signed32);
                mc.MemorySet(x1FilterSecondaryCoefficientAddress, defaultSecondaryCoeff.signed32);

                // setup X2 filter default coefficients
                mc.MemorySet(x2FilterPrimaryCoefficientAddress, defaultPrimaryCoeff.signed32);
                mc.MemorySet(x2FilterSecondaryCoefficientAddress, defaultSecondaryCoeff.signed32);

            }
        }

    }
}