LGES-2170-ACW_CCW-Welder/AuroraDevices/AuroraMotionBoard/Ajin/AJINMotor.cs

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

namespace AuroraMotionBoard.Ajin
{
    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 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 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;
            }

            if (!_ajin.IsServoOn(_AxisNo))
                ServoOn(true);

            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);
        }

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

        internal 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;
        }

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

    }
}