OHV/Dev/OHV/VehicleControlSystem/ControlLayer/Axis/EzAxis.cs

using GSG.NET.Excel;
using GSG.NET.Utils;
using System;
using System.Linq;
using System.Threading;
using VehicleControlSystem.ControlLayer.Lib.EziPlusE;

namespace VehicleControlSystem.ControlLayer.Axis
{
    public class EzAxis : AbstractAxis
    {
        static GSG.NET.Logging.Logger logger = GSG.NET.Logging.Logger.GetLogger();

        public AxisConfig Config { get; set; }
        public override AxisData ValocityData { get; set; }

        public override double CurrentPosition
        {
            get
            {
                int pos = 0;
                EziMOTIONPlusELib.FAS_GetActualPos( Config.AxisNo, ref pos );
                return pos * Config.Scale;
            }
        }
        public double CommandPosition
        {
            get
            {
                int pos = 0;
                EziMOTIONPlusELib.FAS_GetCommandPos( Config.AxisNo, ref pos );
                return pos * Config.Scale;
            }
        }

        public bool IsConnect { get { return EziMOTIONPlusELib.FAS_IsSlaveExist( Config.AxisNo ) == 1 ? true : false; } }

        E_VelocityMode e_VelocityMode = E_VelocityMode.ManualMode;
        public override E_VelocityMode VelocityMode
        {
            get => this.e_VelocityMode;
            set
            {
                if( this.e_VelocityMode == value )
                    return;
                this.e_VelocityMode = value;
            }
        }

        public EzAxis( AxisConfig config )
        {
            this.Config = config;
            this.Tag = config.AxisName;
            this.AxisID = config.AxisNo;
        }

        /// <summary>
        /// 현재 위치를 0으로 함
        /// CommandPos, ActualPos 둘다.
        /// </summary>
        /// <returns></returns>
        public override int SetAbsPosition()
        {
            return EziMOTIONPlusELib.FAS_ClearPosition( Config.AxisNo );
        }

        public override int EStop()
        {
            return EziMOTIONPlusELib.FAS_EmergencyStop( Config.AxisNo );
        }

        public override int GetComVelocity()
        {
            throw new NotImplementedException();
        }

        public override int Initialize()
        {
            throw new NotImplementedException();
        }

        public override int Initialize( AxisData data )
        {
            //IO 에서 연결 시도.
            //EziMOTIONPlusELib.FAS_Connect( (byte)192, (byte)168, (byte)0, (byte)Config.AxisNo, Config.AxisNo );

            this.ValocityData = data;

            //초기에 설정한다.
            this.SetAccelTime( this.ValocityData.AutoRunAccelerate );
            this.SetDecelTime( this.ValocityData.AutoRunDecelerate );

            return 0;
        }

        int GetAllStatus( out uint axisStatus )
        {
            uint axisDIn = 0, axisDOut = 0;
            int cmdPos = 0, actPos = 0, posErr = 0, actVel = 0;
            ushort posItemNo = 0;
            axisStatus = 0;

            return EziMOTIONPlusELib.FAS_GetAllStatus( Config.AxisNo, ref axisDIn, ref axisDOut, ref axisStatus,
                ref cmdPos, ref actPos, ref posErr, ref actVel, ref posItemNo );
        }

        public override bool IsAxisDone()
        {
            return IsInposition() && !IsInMotion() ? true : false;
        }

        public bool IsInposition()
        {
            uint axisStatus;

            if( this.GetAllStatus( out axisStatus ) == EziMOTIONPlusELib.FMM_OK )
            {
                return Convert.ToBoolean( axisStatus & EziSERVO2PlusE.FFLAG_INPOSITION ) ? true : false;
            }

            return false;
        }

        public override bool IsInMotion()
        {
            uint axisStatus;

            if( this.GetAllStatus( out axisStatus ) == EziMOTIONPlusELib.FMM_OK )
            {
                return Convert.ToBoolean( axisStatus & EziSERVO2PlusE.FFLAG_MOTIONING ) ? true : false;
            }

            return false;
        }

        public override bool IsMotionDone()
        {
            uint axisStatus;

            if( this.GetAllStatus( out axisStatus ) == EziMOTIONPlusELib.FMM_OK )
            {
                return Convert.ToBoolean( axisStatus & EziSERVO2PlusE.FFLAG_MOTIONING ) ? false : true;
            }

            return false;
        }

        public override bool IsOriginReturn()
        {
            uint axisStatus;

            if( this.GetAllStatus( out axisStatus ) == EziMOTIONPlusELib.FMM_OK )
            {
                return Convert.ToBoolean( axisStatus & EziSERVO2PlusE.FFLAG_ORIGINRETOK ) ? true : false;
            }

            return false;
        }

        public bool IsOriginReturning()
        {
            uint axisStatus;

            if( this.GetAllStatus( out axisStatus ) == EziMOTIONPlusELib.FMM_OK )
            {
                return Convert.ToBoolean( axisStatus & EziSERVO2PlusE.FFLAG_ORIGINRETURNING ) ? true : false;
            }

            return false;
        }

        public override bool IsServoOn()
        {
            uint axisStatus;

            if( this.GetAllStatus( out axisStatus ) == EziMOTIONPlusELib.FMM_OK )
            {
                return Convert.ToBoolean( axisStatus & EziSERVO2PlusE.FFLAG_SERVOON ) ? true : false;
            }

            return false;
        }

        public override bool IsServoOff()
        {
            uint axisStatus;

            if( this.GetAllStatus( out axisStatus ) == EziMOTIONPlusELib.FMM_OK )
            {
                return Convert.ToBoolean( axisStatus & EziSERVO2PlusE.FFLAG_SERVOON ) ? false : true;
            }

            return false;
        }

        public override bool IsAmpFault()
        {
            uint axisStatus;

            if( this.GetAllStatus( out axisStatus ) == EziMOTIONPlusELib.FMM_OK )
            {
                return Convert.ToBoolean( axisStatus & EziSERVO2PlusE.FFLAG_ERRORALL ) ? true : false;
            }

            return false;
        }

        public bool IsMinusLimit()
        {
            uint axisStatus;

            if( this.GetAllStatus( out axisStatus ) == EziMOTIONPlusELib.FMM_OK )
            {
                return Convert.ToBoolean( axisStatus & EziSERVO2PlusE.FFLAG_HWNEGALMT ) ? true : false;
            }

            return false;
        }

        public bool IsPlusLimit()
        {
            uint axisStatus;

            if( this.GetAllStatus( out axisStatus ) == EziMOTIONPlusELib.FMM_OK )
            {
                return Convert.ToBoolean( axisStatus & EziSERVO2PlusE.FFLAG_HWPOSILMT ) ? true : false;
            }

            return false;
        }

        /// <summary>
        /// Command Pos 와 Current Pos 의 차이를 확인한다.
        /// </summary>
        /// <returns>Tolenrace 범위 내 인지 확인</returns>
        bool IsInTolerance()
        {
            return Math.Abs( CurrentPosition - CommandPosition ) < 0.01 ? true : false;
        }

        /// <summary>
        /// Jog Move
        /// </summary>
        /// <param name="velocity">입력 값이 없으면 속도 5mm/sec </param>
        /// <param name="dir"></param>
        /// <returns></returns>
        public override int JogMove( E_JogMoveDir dir, double dVelocity = 0.0 )
        {
            uint uiVelocity = 0;

            if( dVelocity == 0 )
                uiVelocity = (uint)Convert.ToInt32( 5 / this.Config.Scale );
            else
                uiVelocity = (uint)Convert.ToInt32( dVelocity / this.Config.Scale );


            if( dir == E_JogMoveDir.Positive )
                return EziMOTIONPlusELib.FAS_MoveVelocity( Config.AxisNo, uiVelocity, EziSERVO2PlusE.DIR_INC );
            else
                return EziMOTIONPlusELib.FAS_MoveVelocity( Config.AxisNo, uiVelocity, EziSERVO2PlusE.DIR_DEC );
        }

        public override int JogMoveFast( E_JogMoveDir dir, double dFast = 0 )
        {
            uint uiVelocity = 0;

            if( dFast == 0 )
                uiVelocity = (uint)Convert.ToInt32( this.ValocityData.JogFast / this.Config.Scale );
            else
                uiVelocity = (uint)Convert.ToInt32( dFast / this.Config.Scale );

            if( dir == E_JogMoveDir.Positive )
                return EziMOTIONPlusELib.FAS_MoveVelocity( Config.AxisNo, uiVelocity, EziSERVO2PlusE.DIR_INC );
            else
                return EziMOTIONPlusELib.FAS_MoveVelocity( Config.AxisNo, uiVelocity, EziSERVO2PlusE.DIR_DEC );
        }

        public override int JogMoveSlow( E_JogMoveDir dir, double dSlow = 0 )
        {
            uint uiVelocity = 0;

            if( dSlow == 0 )
                uiVelocity = (uint)Convert.ToInt32( this.ValocityData.JogSlow / this.Config.Scale );
            else
                uiVelocity = (uint)Convert.ToInt32( dSlow / this.Config.Scale );

            if( dir == E_JogMoveDir.Positive )
                return EziMOTIONPlusELib.FAS_MoveVelocity( Config.AxisNo, uiVelocity, EziSERVO2PlusE.DIR_INC );
            else
                return EziMOTIONPlusELib.FAS_MoveVelocity( Config.AxisNo, uiVelocity, EziSERVO2PlusE.DIR_DEC );
        }

        public override int Move( double dPosition, double dVelocity = 0, int iAccelerate = 0, int iDecelerate = 0, int iMoveType = 0 )
        {
            if( !IsOriginReturn() )
                return -1;

            if( StartMove( dPosition, dVelocity, iAccelerate, iDecelerate, iMoveType ) > 0 )
            {
            }
            Thread.Sleep( 300 );

            return Wait4Done();
        }

        public override int StartMove( double dPosition, double dVelocity, int iAccelerate = 0, int iDecelerate = 0, int iMoveType = 0 )
        {
            if( !IsConnect )
                return -1;

            if( !IsServoOn() )
                return -1;

            if( iAccelerate > 0 )
                SetAccelTime( iAccelerate );
            else
                SetAccelTime( this.ValocityData.AutoRunAccelerate );

            if( iDecelerate > 0 )
                SetDecelTime( iDecelerate );
            else
                SetDecelTime( this.ValocityData.AutoRunDecelerate );

            int iPulse = Convert.ToInt32( dPosition / this.Config.Scale );

            uint uiVelocity = 0;
            if( dVelocity == 0 )
            {
                if( this.VelocityMode == E_VelocityMode.AutoMode )
                    uiVelocity = (uint)Convert.ToInt32( this.ValocityData.AutoRunVelocity / this.Config.Scale );
                else
                    uiVelocity = (uint)Convert.ToInt32( this.ValocityData.ManualRunVelocity / this.Config.Scale );
            }
            else
                uiVelocity = (uint)Convert.ToInt32( dVelocity / this.Config.Scale );


            return EziMOTIONPlusELib.FAS_MoveSingleAxisAbsPos( Config.AxisNo, iPulse, uiVelocity );
        }

        public override int Wait4Done( bool bSkipMode = false, bool bUseInterlock = false, bool bRMove = false )
        {
            var sTime = SwUtils.CurrentTimeMillis;

            while( !IsAxisDone() )
            {
                Thread.Sleep( 5 );

                if( SwUtils.Elapsed( sTime ) > 20000 )
                {
                    Stop();
                    return -1;
                }

                if( IsAmpFault() )
                    return -3;
            }

            if( !IsInTolerance() )
                return -2;

            return 0;
        }

        public override int OriginReturn( bool bMove )
        {
            return EziMOTIONPlusELib.FAS_MoveOriginSingleAxis( Config.AxisNo );
        }

        public override int ResetAmpFault()
        {
            return EziMOTIONPlusELib.FAS_ServoAlarmReset( Config.AxisNo );
        }

        public override int RMove( double dDistance, double dVelocity = 0, int iAccelerate = 0, int iDecelerate = 0, int iMoveType = 0, bool bClearOpt = true )
        {
            throw new NotImplementedException();
        }

        public override int ServoOff()
        {
            return EziMOTIONPlusELib.FAS_ServoEnable( Config.AxisNo, 0 );
        }

        public override int ServoOn()
        {
            int result = EziMOTIONPlusELib.FAS_IsSlaveExist(Config.AxisNo);
            if (result != EziMOTIONPlusELib.FMM_OK)
                return result;

            return EziMOTIONPlusELib.FAS_ServoEnable( Config.AxisNo, 1 );
        }

        public override int StartRMove( double dDistance, double dVelocity = 0, int iAccelerate = 0, int iDecelerate = 0, int iMoveType = 0, bool bClearOpt = true )
        {
            throw new NotImplementedException();
        }

        public override int Stop()
        {
            return EziMOTIONPlusELib.FAS_MoveStop( Config.AxisNo );
        }

        public int EmergencyStop()
        {
            return EziMOTIONPlusELib.FAS_EmergencyStop( Config.AxisNo );
        }

        public override int StopReturnOrigin()
        {
            throw new NotImplementedException();
        }

        public override int VMove( double dVelocity = 0, int iAccelerate = 0, bool bDir = true )
        {
            return EziMOTIONPlusELib.FAS_MoveVelocity( Config.AxisNo, (uint)dVelocity, EziSERVO2PlusE.DIR_DEC );
        }

        public override int VMove( double dVelocity = 0, int waitms = 0 )
        {
            uint uiVelocity = 0;

            if( dVelocity == 0 )
            {
                if( this.VelocityMode == E_VelocityMode.AutoMode )
                    uiVelocity = (uint)Convert.ToInt32( this.ValocityData.AutoRunVelocity / this.Config.Scale );
                else
                    uiVelocity = (uint)Convert.ToInt32( this.ValocityData.ManualRunVelocity / this.Config.Scale );
            }
            else
                uiVelocity = (uint)Convert.ToInt32( dVelocity / this.Config.Scale );

            int result = EziMOTIONPlusELib.FAS_MoveVelocity( Config.AxisNo, uiVelocity, EziSERVO2PlusE.DIR_DEC );

            if( waitms == 0 )
                return result;

            return Wait4VMoveDone( waitms ); ;
        }

        public int Wait4VMoveDone( int waitTime )
        {
            var sTime = SwUtils.CurrentTimeMillis;

            while( !IsAxisDone() )
            {
                Thread.Sleep( 5 );

                if( SwUtils.Elapsed( sTime ) > waitTime )
                {
                    Stop();
                    return -1;
                }
            }
            Stop();

            return 0;
        }

        public override int VStop()
        {
            throw new NotImplementedException();
        }


        public int SetParameter( byte paramNo, int paramValue )
        {
            return EziMOTIONPlusELib.FAS_SetParameter( Config.AxisNo, paramNo, paramValue );
        }

        public int SetAccelTime( int accelTime )
        {
            return EziMOTIONPlusELib.FAS_SetParameter( Config.AxisNo, (byte)3, accelTime );
        }

        public int SetDecelTime( int decelTime )
        {
            return EziMOTIONPlusELib.FAS_SetParameter( Config.AxisNo, (byte)4, decelTime );
        }

        public int SaveAllParameters()
        {
            return EziMOTIONPlusELib.FAS_SaveAllParameters( Config.AxisNo );
        }
    }
}