LGES-2170-ACW_CCW-Welder/AuroraDevices/AuroraACS/ACSMotor.cs

using ACS.SPiiPlusNET;
using Aurora.Api;
using Aurora.Api.Devices;
using AuroraMotor;
using Hermes;
using PropertyTools.DataAnnotations;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Axis = ACS.SPiiPlusNET.Axis;

namespace AuroraACS
{
    public class ACSMotor : AuroraMotorBase    // BaseMotor
    {
        private readonly Logger Log = new Logger("ACSMotor");

        public bool IsInit = false;

        public int BufferNo;
        public Axis AxisNo;
        private readonly string AxisName;
        private double TargetPosition;
        private double TargetPositionABS;
        private TargetPosType TargetType;
        ACSDevice ACSLibrary;
        private ACSMotorConfig _Config;
        bool IsHomeComplete = false;

        public ACSMotor(string deviceId, ACSDevice acs, int bufferNo, int axisNo, string axisName = null) : base(deviceId)
        {
            ACSLibrary = acs;
            BufferNo = bufferNo;
            AxisNo = (Axis)axisNo;
            AxisName = axisName;
        }

        protected override int DoActivate(IDeviceConfig config)
        {
            if (IsActivated) return E1.OK;

            _Config = config as ACSMotorConfig;
            if (!ACSLibrary.IsActivated) {
                Log.E("ACS Not activated.");
                return E1.NETWORK;
            }
            if (ACSLibrary.IsSimul) {
                ServoOn(true);
            }
            return E1.OK;
        }

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

            if (!ACSLibrary.IsActivated) {
                return E1.OK;
            }

            IsInit = false;
            IsHomeComplete = false;
            return E1.OK;
        }

        public override int ClearAlarm()
        {
            ACSLibrary.ClearAlam(AxisNo);
            return E1.OK;
        }

        public override bool IsAlarm()
        {
            var state = ACSLibrary.GetMotorAlarmCode(AxisNo);
            var fault = ACSLibrary.GetFault(AxisNo);
            if (state == 0 && fault == 0) {
                return false;
            }

            return true;
        }

        public override IList<string> GetAlarm()
        {
            if (!IsActivated) {
                return null;
            }
            var mask = ACSLibrary.GetMotorAlarmCode(AxisNo);
            if (mask == 0) {
                return null;
            }


            List<string> alarms = new List<string>();
            alarms.Add(ACSLibrary.GetMotorAlarmDesc(mask));

            return alarms;
        }

        public override double GetActualPos()
        {
            return ACSLibrary.ReadMotorPosition(AxisNo);
        }

        public double GetReferencePosition()
        {
            return ACSLibrary.ReadRPosition(AxisNo);
        }

        public override bool IsTargetPosLimit()
        {
            if (_Config == null) return true;
            if (!IsActivated) return true;

            return (TargetPositionABS < _Config.MoveRange.Min) || (TargetPositionABS > _Config.MoveRange.Max);
        }

        public override Range2<double> GetPosLimit()
        {
            if (_Config == null) return null;
            if (!IsActivated) return null;

            return new Range2<double>(_Config.MoveRange.Min, _Config.MoveRange.Max);
        }

        public override double GetTargetPos()
        {
            return TargetPositionABS;
        }

        public override double GetActualVel()
        {
            var vel = ACSLibrary.GetVelocity(AxisNo);
            if (vel < 0) {
                return 0;
            }

            return (uint)vel;
        }

        public double GetFeedbackVelocity()
        {
            return ACSLibrary.GetFeedbackVelocity(AxisNo);
        }

        public override Range<double> GetVelLimit()
        {
            return new Range<double>(_Config.VelocityRange.Min, _Config.VelocityRange.Max);
        }

        public override int Home()
        {
            if (ACSLibrary.IsSimul) {
                ACSLibrary.SetZeroPosition(AxisNo);
                IsHomeComplete = true;
                return E1.OK;
            }

            if (!ACSLibrary.RunBuffer((ProgramBuffer)BufferNo)) {
                Log.E("Motor initiallize fail : " + DeviceId);
                return E1.GENERIC;
            }
            return E1.OK;
        }

        public int Init()
        {
            if (!IsServoOn()) {
                if (ServoOn(true) != E1.OK) {
                    Log.E("ServoOn Error : " + DeviceId);
                    return E1.GENERIC;
                }
            }

            if (ACSLibrary.IsSimul) {
                if (Home() != E1.OK) {
                    return E1.GENERIC;
                }
            }

            IsInit = true;
            return E1.OK;

            //if (!IsInit) {
            //    if (!ACSLibrary.RunBuffer((ProgramBuffer)BufferNo)) {
            //        Log.E("Motor initiallize fail : " + DeviceId);
            //        return E1.GENERIC;
            //    }
            //    IsHomeComplete = true;
            //}

            //return E1.OK;
        }

        public override bool IsHomeCompleted()
        {
            if (ACSLibrary.IsSimul) {
                return true;
            }

            return true;

            //string strInit = "initComp_" + AxisName;
            //return ((ACSLibrary.ReadVariableInt(strInit) == 1) ? true : false);
        }

        public bool IsInitiallized()
        {
            if (ACSLibrary.IsSimul) {
                return IsInit;
            }

            string strInit = "initComp_" + AxisName;
            IsInit = ((ACSLibrary.ReadVariableInt(strInit) == 1) ? true : false);
            return IsInit;
        }

        public override bool IsMoving()
        {
            var state = ACSLibrary.GetMotorState(AxisNo);
            if ((state & MotorStates.ACSC_MST_MOVE) != 0) {
                return true;
            }

            return false;
        }

        public override bool IsServoOn()
        {
            var state = ACSLibrary.GetMotorState(AxisNo);
            if ((state & MotorStates.ACSC_MST_ENABLE) == 0) {
                return false;
            }

            return true;
        }

        public override int Jog(int direction, double velocity)
        {
            return ACSLibrary.Jog(AxisNo, velocity * direction);
        }

        public override int Jog(int direction)
        {
            var jogSpeed = (_Config as ACSMotorConfig).MidJogSpeed;
            if (SelectedJogSpeed == eJogSpeed.L) {
                jogSpeed = (_Config as ACSMotorConfig).LowJogSpeed;
            } else if (SelectedJogSpeed == eJogSpeed.H) {
                jogSpeed = (_Config as ACSMotorConfig).HighJogSpeed;
            }

            //return ACSLibrary.Jog(AxisNo, (GlobalDirection)direction);
            return ACSLibrary.Jog(AxisNo, jogSpeed * direction);
        }

        public override int JogIncrement(double increment, double velocity)
        {
            SetTargetSPD(velocity);
            SetTargetPos(GetActualPos() + increment);
            Move();
            return -1;
        }

        public override int Move()
        {
            if (TargetPositionABS < _Config.MoveRange.Min) {
                Log.E($"Limit Over Error : Requested[{TargetPositionABS}] < - Limit[{_Config.MoveRange.Min}]");
                return -1;
            } else if (TargetPositionABS > _Config.MoveRange.Max) {
                Log.E($"Limit Over Error : Requested[{TargetPositionABS}] > - Limit[{_Config.MoveRange.Max}]");
                return -1;
            }

            if (TargetType == TargetPosType.ABS) {
                return ACSLibrary.MoveABS(AxisNo, TargetPosition);
            } else {
                return ACSLibrary.MoveINC(AxisNo, TargetPosition);
            }
        }

        public int DoMove(int velocity)
        {
            SetTargetSPD(velocity);
            return Move();
        }

        public override int ServoOn(bool onOff)
        {
            ACSLibrary.SetMotorEnable(AxisNo, onOff);
            return E1.OK;
        }

        public override int SetTargetPos(double targetPos, TargetPosType type = TargetPosType.ABS)
        {
            TargetType = type;
            TargetPosition = targetPos;
            if (TargetType == TargetPosType.ABS) {
                TargetPositionABS = TargetPosition;
            } else {
                TargetPositionABS = GetActualPos() + TargetPosition;
            }

            return E1.OK;
        }

        public override int SetTargetSPD(double tarVel)
        {
            if (ACSLibrary.SetVelocity(AxisNo, tarVel)) {
                return E1.OK;
            }
            return E1.ARGUMENT;
        }

        public override double GetTargetSPD()
        {
            return ACSLibrary.GetVelocity(AxisNo);
        }

        private eJogSpeed SelectedJogSpeed = eJogSpeed.M;
        public override int SetJogSPD(eJogSpeed jogSPD)
        {
            SelectedJogSpeed = jogSPD;
            return 0;
        }

        public int SetVelocityImm(double velocity)
        {
            if (ACSLibrary.SetVelocityImm(AxisNo, velocity)) {
                return E1.OK;
            }
            return E1.ARGUMENT;
        }

        public int SetAccelerationImm(double accel)
        {
            if (ACSLibrary.SetAccelerationImm(AxisNo, accel)) {
                return E1.OK;
            }
            return E1.ARGUMENT;
        }

        public int SetAcceleration(double accel)
        {
            if (ACSLibrary.setAcceleration(AxisNo, accel)) {
                return E1.OK;
            }
            return E1.ARGUMENT;
        }

        public double GetAcceleration()
        {
            return ACSLibrary.GetAcceleration(AxisNo);
        }

        public int SetDeceleration(double decel)
        {
            if (ACSLibrary.SetDeceleration(AxisNo, decel)) {
                return E1.OK;
            }
            return E1.ARGUMENT;
        }

        public double GetDeceleration()
        {
            return ACSLibrary.GetDeceleration(AxisNo);
        }

        public int SetDecelerationImm(double decel)
        {
            if (ACSLibrary.SetDecelerationImm(AxisNo, decel)) {
                return E1.OK;
            }
            return E1.ARGUMENT;
        }

        public double GetKillDeceleration()
        {
            return ACSLibrary.GetKillDeceleration(AxisNo);
        }

        public bool SetJerkImm(double dVel)
        {
            if (!IsActivated)
                return false;

            return ACSLibrary.SetJerkImm(AxisNo, dVel);
        }

        public double GetJerk()
        {
            return ACSLibrary.GetJerk(AxisNo);
        }

        public override int Stop()
        {
            ACSLibrary.SetHalt(AxisNo);
            return E1.OK;
        }

        public override int StopJog()
        {
            ACSLibrary.SetHalt(AxisNo);
            return E1.OK;
        }

        public int DoStopAll()
        {
            ACSLibrary.SetHaltAll();
            return E1.OK;
        }

        public int EmergencyStop()
        {
            ACSLibrary.SetKill(AxisNo);
            return E1.OK;
        }

        public virtual bool ReadInposition()
        {
            var state = ACSLibrary.GetMotorState(AxisNo);
            if ((state & MotorStates.ACSC_MST_INPOS) != 0) {
                return true;
            }

            return false;
        }

        public bool IsAccelerating()
        {
            var state = ACSLibrary.GetMotorState(AxisNo);
            if ((state & MotorStates.ACSC_MST_ACC) != 0) {
                return true;
            }

            return false;
        }

        public bool IsHPLimit()
        {
            return ACSLibrary.IsHPLimit(AxisNo);
        }


        public bool IsHNLimit()
        {
            return ACSLibrary.IsHNLimit(AxisNo);
        }

        public bool IsSPLimit()
        {
            return ACSLibrary.IsSPLimit(AxisNo);
        }

        public bool IsSNLimit()
        {
            return ACSLibrary.IsSNLimit(AxisNo);
        }


        public void RunBuffer(string label, Action<int> programEnd)
        {
            ACSLibrary.RunBuffer((ProgramBuffer)BufferNo, label, programEnd);
        }

        public override bool IsInpos()
        {
            return ACSLibrary.ReadInposition(AxisNo);
        }
        public double GetCurrentOverload()
        {
            return ACSLibrary.GetAxisCurrentOverload(AxisNo);
        }
    }
}