OHV/Dev/OHV/VehicleControlSystem/ControlLayer/Motion/GSIDrive.cs

using GSG.NET.Concurrent;
using GSG.NET.Extensions;
using GSG.NET.Logging;
using GSG.NET.Utils;
using OHV.Common.Model;
using OHV.Common.Shareds;
using OHV.SqliteDAL;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using VehicleControlSystem.ControlLayer.MQ;
using VehicleControlSystem.Managers;

namespace VehicleControlSystem.ControlLayer.Drive
{
    public class GSIDrive : ControlObjectBase
    {
        public enum eDriveServoState
        {
            ServoFault = -1,
            WrongServoNum = 0,
            ServoOn = 1,
            ServoOff = 2,
            ServoStop = 3,
            None,
        }

        Logger logger = Logger.GetLogger();

        public bool IsStop { get; set; }

        double currentPos = 0;
        public double CurrentPos { get { return this.currentPos; } set { SetField( ref this.currentPos, value ); } }

        private int currentTag;
        public int CurrentTag { get { return currentTag; } set { SetField( ref this.currentTag, value ); } }

        private double currentSpeed = 0;
        public double CurrentSpeed { get { return currentSpeed; } set { SetField( ref this.currentSpeed, value ); } }

        private double currentTorque = 0;
        public double CurrentTorque { get { return currentTorque; } set { SetField( ref this.currentTorque, value ); } }

        public bool IsErrorOn { get; set; }

        private eDriveServoState driveServoState;
        public eDriveServoState DriveServoState
        {
            get { return driveServoState; }
            set { SetField( ref driveServoState, value ); }
        }

        TaskCancel taskCancel = new TaskCancel();
        ThreadCancel threadCancel = new ThreadCancel();
        SqliteManager sql = null;

        Steering steering = null;

        ZmqManager zmq = null;

        List<Alarm> Alarms { get; set; }

        //이후 상태가 불일치가 나온다면 필요시 마다 요구 하는 방식으로 변경.
        public bool IsDriveFault { get; set; } = false;

        public GSIDrive( SqliteManager sql, Steering steering, ZmqManager zmq, List<Alarm> alarms )
        {
            this.sql = sql;
            this.steering = steering;
            this.zmq = zmq;

            this.Alarms = alarms;
        }

        private void Zmq_OnDriveAlarmBitArrayChg(System.Collections.BitArray obj)
        {
            bool isFault = false;
            for (int i = 0; i < obj.Count; i++)
            {
                if (obj[i])
                {
                    var alarm = this.Alarms.Where(a => a.AlarmId == i + 1000).SingleOrDefault();
                    if (alarm == null)
                    {
                        logger.D($"[Drive] - AlarmID [ {i+1000} ] - is Null");
                        continue;
                    }

                    if ( alarm.Level == eAlarmLevel.Fault )
                        isFault = true; //알람 중 하나라도 Fault 가 있으면 상태 Fault.
                }
            }

            if (isFault)
                this.IsDriveFault = true;
            else
                this.IsDriveFault = false;
        }

        public void Init()
        {
            //this.threadCancel.AddGo( Thread_DriveStateChcker );
            //this.threadCancel.AddGo( Thread_Logger );

            //zmq.PropertyChanged += Zmq_PropertyChanged;
            this.zmq.OnDriveAlarmBitArrayChg += Zmq_OnDriveAlarmBitArrayChg;

            //PhysicalCheckupLogger.Instance.Connecte();
        }

        private void Zmq_PropertyChanged( object sender, System.ComponentModel.PropertyChangedEventArgs e )
        {
            var property = sender.GetType().GetProperty( e.PropertyName );
            var newValue = property.GetValue( sender, null );

            switch ( property.Name )
            {
                //case "RequestSteering":
                //    {
                //        var v = CastTo<eSteeringState>.From<object>( newValue );
                //        this.ReqSteeringState = v;
                //    }
                //    break;

                //case "CurrentPointNo":
                //    {
                //        var v = CastTo<int>.From<object>( newValue );
                //        this.CurrentTag = v;
                //    }
                //    break;
                //case "RearLoadFactor":
                //    {
                //        var v = CastTo<double>.From<object>( newValue );
                //        this.RearLoadFactor = v;
                //    }
                //    break;
                //case "RearRPM":
                //    {
                //        var v = CastTo<double>.From<object>( newValue );
                //        this.RearRpm = v;
                //    }
                //    break;
                //case "FrontLoadFactor":
                //    {
                //        var v = CastTo<double>.From<object>( newValue );
                //        this.FrontLoadFactor = v;
                //    }
                //    break;
                //case "FrontRPM":
                //    {
                //        var v = CastTo<double>.From<object>( newValue );
                //        this.FrontRpm = v;
                //    }
                //    break;

                default:
                    break;
            }
        }
        public void Dispose()
        {
            this.threadCancel.Cancel();
            LockUtils.Wait( 50 );
            this.threadCancel.StopWaitAll();
        }

        void Thread_DriveStateChcker()
        {
            logger.D( "[Drive] - Thread Drive State Checker Start" );

            while ( !this.threadCancel.Canceled )
            {
                try
                {
                    LockUtils.Wait( 10 );
                    //this.GetReqSteeringState();
                    //if (Redis.Instance.GetDriveMove() )
                    //{
                    //    this.LoggingState();
                    //}
                }
                catch ( Exception e )
                {
                    logger.E( $"[Drive] - Error : [{e}]" );
                }
            }

            logger.D( "[Drive] - Thread Drive State Checker Disposed" );
        }

        void Thread_Logger()
        {
            while ( !this.threadCancel.Canceled )
            {
                try
                {
                    LockUtils.Wait( 10 );
                }
                catch ( Exception e )
                {
                    logger.E( $"[Drive] - Error : [{e}]" );
                }
            }
        }

        public void SetCurrentSteeringState( eSteeringState state )
        {
            this.zmq.SetCurrentSteeringState( state );
        }

        public bool IsDriveStop() => !zmq.IsDriveMoving;
        public bool IsDriveMoving() => zmq.IsDriveMoving;

        /// <summary>
        /// Drive 측 초기화 기능
        /// 실패 시 알람 처리
        /// </summary>
        /// <returns></returns>
        public int DriveInit( )
        {
            int result = ConstInt.EXECUTE_SUCCESS;

            if (zmq.DriveState == eDriveState.Fault)
            {
                zmq.SetDriveControl(eDriveControl.ResetFault);
                LockUtils.Wait(1000);
            }

            if (zmq.DriveState == eDriveState.Fault || IsDriveFault)
                return 45;

            //Init Request
            result = this.zmq.SetDriveControl(eDriveControl.Initialize);
            if (result != ConstInt.EXECUTE_SUCCESS )
                return result;

            long sTime = SwUtils.CurrentTimeMillis;
            while (true)
            {
                LockUtils.Wait(5);

                if ( SwUtils.Gt(sTime, 30 * ConstUtils.ONE_SECOND))
                {
                    return 46;
                }

                //Initialize 완료 확인
                if (zmq.Isinitialized)
                    break;
            }

            return result;
        }

        public eOperatationMode GetDriveOperationMode()
        {
            //var ret = Redis.Instance.GetSystemState();

            //if ( ret == 3 )
            //    return eOperatationMode.ManualMode;
            //else if ( ret == 4 )
            //    return eOperatationMode.AutoMode;
            //else

            return eOperatationMode.InitialMode;
        }

        public int MoveToPoint( SubCmd sub, double velocity )
        {
            //if ( this.DriveServoState != eDriveServoState.ServoStop )
            //    return 9999;

            //Drive Fault 상태 확인
            if ( this.IsDriveFault )
                return 45;

            int result = ConstInt.EXECUTE_SUCCESS;

            var steeringState = steering.GetSteeringState();
            if (steeringState == eSteeringState.None ) //현재 가 None 이면 기다렸다가 다시 확인 필요.
                LockUtils.Wait( 500 );

            //zmq.SetCurrentSteeringState( steering.GetSteeringState() );
            LockUtils.Wait( 200 );

            StringBuilder sb = new StringBuilder();
            if ( sub.TargetList.Count <= 0 )
            {
                sb.Append( sub.TargetID );
                result = zmq.SetTargetPoint( sb.ToString() );
                LockUtils.Wait( 100 );
                if ( result != ConstInt.EXECUTE_SUCCESS )
                    return result;

                result = zmq.SetStartMove();
                if ( result != ConstInt.EXECUTE_SUCCESS )
                    return result;
            }
            else
            {
                sub.TargetList.ForEach( x => { sb.Append( x ); sb.Append( ";" ); } );
                result = zmq.SetTargetPointArray( sb.ToString() );
                LockUtils.Wait( 100 );
                if ( result != ConstInt.EXECUTE_SUCCESS )
                    return result;

                result = zmq.SetStartMove(true);
                if ( result != ConstInt.EXECUTE_SUCCESS )
                    return result;
            }

            //result = zmq.SetStartMove();
            //if ( result != ConstInt.EXECUTE_SUCCESS )
            //    return result;

            //10sec 동안 출발 하는지 확인.
            int waitTime = 10000;
            long st = SwUtils.CurrentTimeMillis;
            while ( true )
            {
                LockUtils.Wait( 10 );

                if ( SwUtils.Gt( st, waitTime ) )
                {
                    logger.D( "Move Start Time Out In 10sec!" );
                    return 37;
                }

                if ( zmq.IsDriveMoving )
                    break;
            }

            return ConstInt.EXECUTE_SUCCESS;
        }

        public void CancelCommand(SubCmd subCmd)
        {

        }

        /// <summary>
        /// 급 정지 사용.
        /// </summary>
        public void EStop()
        {
            this.zmq.SetEStop();
        }

        /// <summary>
        /// Manual 상태 에서 가감속 Stop
        /// </summary>
        public void Stop()
        {
            this.zmq.SetDecelStop();
        }

        public void SetObstacleState( eObstacleState state ) => zmq.SetObstruction( state );

        public bool VelocityChainge( double velocity )
        {
            return true;
        }

        public bool JogForword()
        {
            this.zmq.SetJogMove();
            return true;
        }
        public bool JogBackword()
        {
            this.zmq.SetJogMove( false );
            return true;
        }

        #region Test Method

        #endregion
    }
}