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

using GSG.NET.Concurrent;
using GSG.NET.Logging;
using OHV.Common.Shareds;
using OHV.SqliteDAL;
using System;
using VehicleControlSystem.ControlLayer.DB;
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 string currentTag;
        public string 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; }

        eSteeringState reqSteeringState = eSteeringState.None;
        public eSteeringState ReqSteeringState { get { return this.reqSteeringState; } set { SetField( ref this.reqSteeringState, value ); } }


        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;

        public GSIDrive( SqliteManager sql )
        {
            this.sql = sql;
        }

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

            PhysicalCheckupLogger.Instance.Connecte();
        }

        public void Dispose()
        {
            Redis.Instance.Dispose();

            this.threadCancel.Cancel();
            LockUtils.Wait( 50 );
            this.threadCancel.StopWaitAll();

            PhysicalCheckupLogger.Instance.Dispose();
        }

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

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

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

        bool isLoggingStart = false;
        public bool IsloggingStart
        {
            get => this.isLoggingStart;
            set
            {
                if ( SetField( ref this.isLoggingStart, value ) )
                {
                    if ( value )
                        PhysicalCheckupLogger.Instance.SetPLCStartDrive();
                    else
                        PhysicalCheckupLogger.Instance.ResetPLCStartDrive();
                }
            }
        }
        void Thread_Logger()
        {
            while ( !this.threadCancel.Canceled )
            {
                try
                {
                    LockUtils.Wait( 10 );

                    if ( Redis.Instance.GetDriveMove() )
                    {
                        this.IsloggingStart = true;
                        this.LoggingState();
                    }
                    else
                        this.IsloggingStart = false;
                }
                catch ( Exception e )
                {
                    logger.E( $"[Drive] - Error : [{e}]" );
                }
            }
        }

        void LoggingState()
        {
            var currentBCR = Redis.Instance.CurrentBCRValue();
            var speed = Redis.Instance.ActualVelocityToSpeed();

            var fTorque = Redis.Instance.TorqueFront();
            var fRPM = Redis.Instance.ActualVelocityToFrontRPM();
            var fLoadFacter = Redis.Instance.LoadFacterFront();
            PhysicalCheckupLogger.Instance.FrontWheelLogging( speed.ToString(), fTorque.ToString(), fRPM.ToString(), fLoadFacter.ToString(), currentBCR.ToString() );

            var rTorque = Redis.Instance.TorqueRear();
            var rRPM = Redis.Instance.ActualVelocityToRearRPM();
            var rLoadFacter = Redis.Instance.LoadFacterRear();
            PhysicalCheckupLogger.Instance.RearWheelLoggging( speed.ToString(), rTorque.ToString(), rRPM.ToString(), rLoadFacter.ToString(), currentBCR.ToString() );
        }

        void GetReqSteeringState()
        {
            var ret = Redis.Instance.GetSteering();
            if ( ret == 1 )// Left
                this.ReqSteeringState = eSteeringState.Left;
            else if ( ret == -1 ) //Right
                this.ReqSteeringState = eSteeringState.Right;
            else if ( ret == 0 ) // Do Nothing
                this.ReqSteeringState = eSteeringState.None;
            else 
            { }
        }

        bool IsDriveFault()
        {
            var ret = Redis.Instance.GetSystemState();
            return ret == 2 ? true : false;
        }

        public void SetDriveOperationMode( eOperatationMode mode )
        {

            if ( mode == eOperatationMode.ManualMode )
                Redis.Instance.SetSystemOperation( 3 );
            else if ( mode == eOperatationMode.AutoMode )
                Redis.Instance.SetSystemOperation( 2 );
            else { }
        }

        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( string point, double velocity )
        {
            //if ( this.DriveServoState != eDriveServoState.ServoStop )
            //    return 9999;

            //Drive Fault 상태 확인
            if ( IsDriveFault() )
                return 34;

            //ToDo: 이동 명령 실행.
            int result = 0;
            if (! Redis.Instance.RouteMapMoveTo( point ) )
            {
                //Todo: Error 처리
            }


            return 0;
        }

        /// <summary>
        /// 급정시 사용.
        /// </summary>
        //public void EStop() => OHVdriveSetServoCommand(3);

        /// <summary>
        /// Manual 상태에서 가감속 Stop
        /// </summary>
        public void Stop()
        {
        }

        //public void JogForWard() => OHVdriveSetManualOperationCommand( 1 );
        //public void JogBackward() => OHVdriveSetManualOperationCommand( -1 );
        //public void ServoOn() => OHVdriveSetServoCommand( 1 );
        //public void ServoOff() => OHVdriveSetServoCommand( 2 );
        //public void AmpFaultReset() => OHVdriveSetServoCommand( -1 );

        public void SetObstacleState( eObstacleState state )
        {
            int field = 0;

            switch ( state )
            {
                case eObstacleState.Normal:
                    field = 0;
                    break;
                case eObstacleState.Abnormal:
                    field = 1;
                    break;
                case eObstacleState.Blocked:
                    field = 2;
                    break;
                case eObstacleState.Decelerate:
                    field = 3;
                    break;
                default:
                    break;
            }
        }

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


#region Test Method

#endregion
    }
}