LGES-OC5-PtoP-Welder/AuroraDevices/AuroraMotionBoard/Ajin/AJINMotor.cs

using Aurora;
using Aurora.Api;
using Aurora.Api.Devices;
using Hermes;
using System;
using System.Collections.Generic;

namespace AuroraMotionBoard.Ajin
{
    public enum JogDirection
    {
        plus = 1,
        minus = -1,
    }
    public enum Speed
    {
        L,
        M,
        H
    }
    public class AJINMotor : BaseMotor
    {
        private readonly Logger Log = new Logger("AjinMotor");
        private AJINDevice _ajin;

        private int _AxisNo;
        private double TargetPosition;
        private TargetPosType TargetType;

        public AJINMotorConfig _Config;
        private double _Speed;
        private double _Acc;
        private double _Dec;

        public AJINMotor(string deviceId, int axisNo, AJINDevice ajin) : base(deviceId)
        {
            _AxisNo = axisNo;
            _ajin = ajin;
        }

        public bool? IsInit { get; internal set; }

        public override int ClearAlarm()
        {
            _ajin.AlarmReset(_AxisNo);
            return E1.OK;
        }

        public override int DoHome()
        {
            //SetHomeVelocity, SetHomeMethod는 Mot 파일에 셋팅 된 파라미터로 호밍 동작이 가능하므로 주석 처리 하겠음

            //int ret = _ajin.SetHomeVelocity(_AxisNo, _Config.VelFirst, _Config.VelSecond, _Config.VelThird, _Config.VelLast, _Config.AccFirst, _Config.AccSecond);
            //if (ret == E2.ABORTED)
            //{
            //    return ret;
            //}
            //ret = _ajin.SetHomeMethod(_AxisNo, _Config.HomeDir, (uint)AXT_MOTION_HOME_DETECT.HomeSensor, (uint)AXT_USE.DISABLE, _Config.EncoderClearDelayTime, 0.0);
            //if (ret == E2.ABORTED)
            //{
            //    return ret;
            //}

            return _ajin.StartHome(_AxisNo);
        }

        public override int DoJog(int direction, double velocity)
        {
            //Jog에서는 Config의 acc, dec를 쓰자..
            return _ajin.MoveJog(_AxisNo, direction, velocity, _Config.Acceleration, _Config.Deceleration);
        }
        public int Jog(JogDirection dir, Speed speed)
        {
            double spd;
            switch (speed)
            {
                case Speed.L:
                    spd = _Config.LowJogSpeed;
                    break;
                case Speed.M:
                    spd = _Config.MidJogSpeed;
                    break;
                case Speed.H:
                    spd = _Config.HighJogSpeed;
                    break;
                default:
                    spd = _Config.LowJogSpeed;
                    break;
            }
            return DoJog((int)dir, spd);
        }
        public override int DoJogIncrement(double increment, double velocity)
        {
            return E1.OK;
        }

        public override int DoMove()
        {
            return TargetType == TargetPosType.ABS ?
                _ajin.MoveABS(_AxisNo, TargetPosition, _Speed, _Acc, _Dec) :
                _ajin.MoveREL(_AxisNo, TargetPosition, _Speed, _Acc, _Dec);
        }
        public int Move(double CommandPosition, Speed spd = Speed.L, TargetPosType type = TargetPosType.ABS)
        {
            SetTargetPosition(CommandPosition, type);
            switch (spd)
            {
                case Speed.L:
                    SetVelocity(_Config.LowMoveSpeed, _Config.LowMoveAcc, _Config.LowMoveAcc);
                    break;
                case Speed.M:
                    SetVelocity(_Config.MidMoveSpeed, _Config.MidMoveAcc, _Config.MidMoveAcc);
                    break;
                case Speed.H:
                    SetVelocity(_Config.HighMoveSpeed, _Config.HighMoveAcc, _Config.HighMoveAcc);
                    break;
                default:
                    break;
            }

            return DoMove();
        }

        public override int DoStop()
        {
            return _ajin.Stop(_AxisNo);
        }

        public override IList<string> GetAlarm()
        {
            throw new NotImplementedException();
        }

        public override double GetCurrentPosition()
        {
            return _ajin.GetCurrentPosition(_AxisNo);
        }

        public override Range<double> GetMoveRange()
        {
            return _Config.MoveRange;
        }

        public override double GetTargetPosition()
        {
            return TargetPosition;
        }

        public override uint GetVelocity()
        {
            return (uint)_Config.DefaultVelocity;
        }

        public override Range<uint> GetVelocityRange()
        {
            return _Config.VelocityRange;
        }

        public override bool IsAlarm()
        {
            if (_ajin.GetAlarmStatus(_AxisNo))
                return true;
            return false;
        }

        public override bool IsHomeCompleted()
        {
            uint status = 0;
            status = _ajin.GetHomeResult(_AxisNo);
            if (status != (uint)AXT_MOTION_HOME_RESULT.HOME_SUCCESS) {
                //Log.E($"Result : {status}");
                return false;
            }
            return true;
        }

        public override bool IsMoving()
        {
            return _ajin.ReadInMotion(_AxisNo);
        }

        public override bool IsServoOn()
        {
            return _ajin.IsServoOn(_AxisNo);
        }

        public override int ServoOn(bool onOff)
        {
            bool Ok = _ajin.ServoOnOff(_AxisNo, onOff ? (uint)AXT_MOTION_SIGNAL_LEVEL.ACTIVE : (uint)AXT_MOTION_SIGNAL_LEVEL.INACTIVE);
            return Ok ? E1.OK : E2.ABORTED;
        }

        public override int SetTargetPosition(double targetPos, TargetPosType type = TargetPosType.ABS)
        {
            TargetPosition = targetPos;
            TargetType = type;
            return E1.OK;
        }
        public int SetVelocity(double speed, double acc, double dec)
        {
            _Speed = speed;
            _Acc = acc;
            _Dec = dec;
            return E1.OK;

        }


        public override int SetVelocity(uint level)
        {
            throw new NotImplementedException();
        }

        protected override int DoActivate(IDeviceConfig config)
        {
            _Config = config as AJINMotorConfig;
            if (!_ajin.IsActivated) {
                Log.E("AJIN Not activated.");
                return E1.NETWORK;
            }

            // Motion Param Setting File Load
            if (!string.IsNullOrEmpty(_ajin._config.MotionSettingsPath)) {
                var res = CAXM.AxmMotLoadParaAll(_ajin._config.MotionSettingsPath);
                if (res != (uint)AXT_FUNC_RESULT.AXT_RT_SUCCESS) {
                    Log.E($"AXM AxmMotLoadParaAll Error : {Enum.GetName(typeof(AXT_FUNC_RESULT), res)}");
                    return E2.ABORTED;
                }
            }

            return Init();
        }

        public int Init()
        {
            var IsServoOn = _ajin.IsServoOn(_AxisNo);
            if(!IsServoOn)
                ServoOn(!IsServoOn);
            return E1.OK;
        }

        protected override int DoDeactivate()
        {
            if (!_ajin.IsActivated) {
                return E1.OK;
            }
            return E1.OK;
        }

        internal void EmergencyStop()
        {
            _ajin.Stop(_AxisNo);
        }

        internal double GetFeedbackVelocity()
        {
            //Target 속도 말고 현재 속도 받아오는 함수가 있는지 확인
            return _ajin.ReadCurrentVelocity(_AxisNo);
        }

        public bool IsHWNegLimit()
        {
            return _ajin.ReadLimit(_AxisNo).HwNeg;
        }

        public bool IsHWPosLimit()
        {

            return _ajin.ReadLimit(_AxisNo).HwPos;
        }

        public uint MotorReScan()
        {
            return _ajin.SSCNET_DeviceRescan(_AxisNo);
        }

        internal bool IsSWNegLimit()
        {
            double currPos = _ajin.GetCurrentPosition(_AxisNo);
            if (double.IsNaN(currPos))
                return true;

            return currPos < _Config.SWLimitMinus;
        }

        internal bool IsSWPosLimit()
        {
            double currPos = _ajin.GetCurrentPosition(_AxisNo);
            if (double.IsNaN(currPos))
                return true;
            return currPos > _Config.SWLimitPlus;
        }

        public bool ReadInposition()
        {
            return _ajin.ReadInPosition(_AxisNo);
        }

        public bool IsReachedTargetPosition(double targetPosition) {
            if (ApplicationExt.IsSimulation)
                return true;

            if (!IsActivated)
                return false;

            if (Math.Abs(GetCurrentPosition() - targetPosition) < 0.05 && ReadInposition())
                return true;

            return false;
        }
    }
}