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

        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;

        Steering steering = null;

        ZmqManager zmq = null;

        public GSIDrive( SqliteManager sql, Steering steering, ZmqManager zmq )
        {
            this.sql = sql;
            this.steering = steering;
            this.zmq = zmq;
        }

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

            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;

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

        bool IsDriveFault()
        {
            return false;
        }

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

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

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

            //ToDo: 이동 명령 실행.
            int result = ConstInt.EXECUTE_SUCCESS;

            zmq.SetCurrentSteeringState( steering.GetSteeringState() );
            LockUtils.Wait( 100 );

            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;

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

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

                if ( zmq.IsDriveMoving )
                    break;
            }

            return ConstInt.EXECUTE_SUCCESS;
        }

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