OHV/Dev/OHV/VehicleControlSystem/ControlLayer/Steering.cs

using System;
using System.Threading.Tasks;
using GSG.NET.Logging;
using GSG.NET.Utils;
using OHV.Common.Shareds;
using OHV.SqliteDAL;
using Prism.Events;
using VehicleControlSystem.ControlLayer.IO;

namespace VehicleControlSystem.ControlLayer
{
    public class Steering : ControlObjectBase
    {
        static Logger logger = Logger.GetLogger();

        EzIO iO = null;
        SqliteManager sql = null;
        IEventAggregator eventAggregator = null;

        public event EventHandler<int> OnSteeringError;

        private eSteeringState steeringState;

        public eSteeringState SteeringState
        {
            get { return steeringState; }
            set { SetField( ref this.steeringState , value ); }
        }

        public Steering( IIO io , SqliteManager sql , IEventAggregator ea )
        {
            this.iO = io as EzIO;
            this.sql = sql;
            this.eventAggregator = ea;
            this.iO.OnChangedIO += IO_OnChangedIO;
        }

        private void IO_OnChangedIO( BitBlock bit )
        {
            if ( bit.Tag.Equals( "IN_F_STEERING_DETECT_LEFT" ) || bit.Tag.Equals( "IN_F_STEERING_DETECT_RIGHT" ) )
                this.GetSteeringState();
            if ( bit.Tag.Equals( "IN_R_STEERING_DETECT_LEFT" ) || bit.Tag.Equals( "IN_R_STEERING_DETECT_RIGHT" ) )
                this.GetSteeringState();
        }

        void GetSteeringState( )
        {
            eSteeringState frontState = eSteeringState.None;
            if ( this.IsFrontLeft() && this.IsFrontRight() )
                frontState = eSteeringState.None;
            if ( !this.IsFrontLeft() && !this.IsFrontRight() )
                frontState = eSteeringState.None;

            if ( this.IsFrontLeft() && !this.IsFrontRight() )
                frontState = eSteeringState.Left;
            if ( !this.IsFrontLeft() && this.IsFrontRight() )
                frontState = eSteeringState.Right;

            eSteeringState rearState = eSteeringState.None;
            if ( this.IsRearLeft() && this.IsRearRight() )
                rearState = eSteeringState.None;
            if ( !this.IsRearLeft() && !this.IsRearRight() )
                rearState = eSteeringState.None;

            if ( this.IsRearLeft() && !this.IsRearRight() )
                rearState = eSteeringState.Left;
            if ( !this.IsRearLeft() && this.IsRearRight() )
                rearState = eSteeringState.Right;

            if ( frontState == eSteeringState.Left && rearState == eSteeringState.Left )
                this.SteeringState = eSteeringState.Left;
            else if ( frontState == eSteeringState.Right && rearState == eSteeringState.Right )
                this.SteeringState = eSteeringState.Right;
            else
                this.SteeringState = eSteeringState.None;
        }

        object lockObj = new object();
        bool isExecuteSteering = false;
        public int ControlSteering( bool isLeft = false )
        {
            if ( this.isExecuteSteering )
                return 0;

            this.isExecuteSteering = true;

            if ( isLeft ) //Left IO 가 직진 이라고 생각하자.
            {
                this.iO.WriteOutputIO( "OUT_F_STEERING_CWCCW" , false );
                this.iO.WriteOutputIO( "OUT_R_STEERING_CWCCW" , true );
                this.SetHightVolt();
                if ( this.SteeringState == eSteeringState.Left )
                {
                    this.isExecuteSteering = false;
                    return 0;
                }
            }
            else
            {
                this.iO.WriteOutputIO( "OUT_F_STEERING_CWCCW" , true );
                this.iO.WriteOutputIO( "OUT_R_STEERING_CWCCW" , false );
                this.SetHightVolt();
                if ( this.SteeringState == eSteeringState.Right )
                {
                    this.isExecuteSteering = false;
                    return 0;
                }
            }

            var task = Task<bool>.Run( ( ) =>
             {
                 long sTime = SwUtils.CurrentTimeMillis;
                 while ( true )
                 {
                     if ( SwUtils.Gt( sTime , 5 * ConstUtils.ONE_SECOND ) ) //Wait 20Sec
                         return false;

                     if ( isLeft )
                     {
                         if ( this.IsFrontLeft() && this.IsRearLeft() )
                             break;
                     }
                     else
                     {
                         if ( this.IsFrontRight() && this.IsRearRight() )
                             break;
                     }
                 }
                 return true;
             } ).ContinueWith( t =>
                  {
                      if ( !t.Result )
                          this.OnSteeringError?.BeginInvoke( this , 33 , null , null );
                      else
                          this.OnSteeringError?.BeginInvoke( this , 0 , null , null );

                      GetSteeringState();
                      this.isExecuteSteering = false;
                  } );

            return 0;
        }

        void SetHightVolt( )
        {
            this.iO.WriteOutputIO( "OUT_F_STEERING_DA" , true );
            this.iO.WriteOutputIO( "OUT_R_STEERING_DA" , true );
            this.iO.WriteOutputIO( "OUT_F_STEERING_DA" , false , 500 );
            this.iO.WriteOutputIO( "OUT_R_STEERING_DA" , false , 500 );
        }

        bool IsFrontLeft( ) => this.iO.IsOn( "IN_F_STEERING_DETECT_LEFT" );
        bool IsFrontRight( ) => this.iO.IsOn( "IN_F_STEERING_DETECT_RIGHT" );
        bool IsRearLeft( ) => this.iO.IsOn( "IN_R_STEERING_DETECT_LEFT" );
        bool IsRearRight( ) => this.iO.IsOn( "IN_R_STEERING_DETECT_RIGHT" );

        public bool IsLeft( ) => this.IsFrontLeft() && this.IsRearLeft();
        public bool IsRight( ) => this.IsFrontRight() && this.IsRearRight();
    }
}