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 VehicleControlSystem.ControlLayer.DB;
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; }

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

        private DriveState frontDriveState;

        public DriveState FrontDriveState
        {
            get { return frontDriveState; }
            set { SetField(ref this.frontDriveState, value); }
        }

        private DriveState rearDriveState;

        public DriveState RearDriveState
        {
            get { return rearDriveState; }
            set { SetField( ref this.rearDriveState, value ); }
        }


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

        ZmqManager zmq = new ZmqManager();

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

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

            zmq.Init();
            zmq.PropertyChanged += Zmq_PropertyChanged;

            //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;
                default:
                    break;
            }
        }

        public void Dispose()
        {
            this.threadCancel.Cancel();
            LockUtils.Wait( 50 );
            this.threadCancel.StopWaitAll();

            zmq.Dispose();
        }

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

        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 fState = new DriveState();
            fState.DirveName = "Front";
            fState.Speed = speed;
            fState.Torque = fTorque;
            fState.Rpm = fRPM;
            fState.Loadage = fLoadFacter;
            this.FrontDriveState = fState;

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

            var rState = new DriveState();
            rState.DirveName = "Rear";
            rState.Speed = speed;
            rState.Torque = rTorque;
            rState.Rpm = rRPM;
            rState.Loadage = rLoadFacter;
            this.RearDriveState = rState;
        }

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

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

        public bool IsDriveStop() => !Redis.Instance.GetDriveMove();
        public bool IsDriveMoving() => Redis.Instance.GetDriveMove();

        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 처리
            //}

            int waitTime = 10000;
            long st = SwUtils.CurrentTimeMillis;
            while ( true )
            {
                LockUtils.Wait( 10 );

                if ( SwUtils.Gt( st, waitTime ) )
                {
                    logger.D("MoveToPoint Time Out");
                    return -1;
                }

                if ( Redis.Instance.GetDriveMove() )
                    break;
            }

            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 ) => zmq.SetObstruction( state );

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

#region Test Method

#endregion
    }
}