|
|
@@ -1,4 +1,5 @@
|
|
|
using System;
|
|
|
+using System.Security.Principal;
|
|
|
using System.Threading.Tasks;
|
|
|
using GSG.NET.Concurrent;
|
|
|
using GSG.NET.Logging;
|
|
|
@@ -8,16 +9,22 @@ using OHV.Common.Shareds;
|
|
|
using OHV.SqliteDAL;
|
|
|
using Prism.Events;
|
|
|
using VehicleControlSystem.ControlLayer.IO;
|
|
|
+using VehicleControlSystem.ControlLayer.MQ;
|
|
|
|
|
|
namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
|
- public class Steering : ControlObjectBase
|
|
|
+ public class Steering : ControlObjectBase, IDisposable
|
|
|
{
|
|
|
static Logger logger = Logger.GetLogger();
|
|
|
+ static Logger SteerLogger = Logger.GetLogger("SteerLogger");
|
|
|
+
|
|
|
+ ThreadCancel threadCancel = new ThreadCancel();
|
|
|
+ TsQueue<eSteeringState> qReqSteer = new TsQueue<eSteeringState>();
|
|
|
|
|
|
EzIO iO = null;
|
|
|
SqliteManager sql = null;
|
|
|
IEventAggregator eventAggregator = null;
|
|
|
+ ZmqManager zmqManager = null;
|
|
|
|
|
|
public event EventHandler<int> OnSteeringError;
|
|
|
|
|
|
@@ -26,17 +33,127 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
public eSteeringState SteeringState
|
|
|
{
|
|
|
get { return steeringState; }
|
|
|
- set { SetField(ref this.steeringState, value); }
|
|
|
+ set { if (SetField(ref this.steeringState, value)) { /*this.zmqManager.SetCurrentSteeringState(value);*/ } }
|
|
|
+ }
|
|
|
+
|
|
|
+ public Steering() { }
|
|
|
+
|
|
|
+ public void Dispose()
|
|
|
+ {
|
|
|
+ this.threadCancel.Cancel();
|
|
|
}
|
|
|
|
|
|
- public Steering(IIO io, SqliteManager sql, IEventAggregator ea)
|
|
|
+ public void AssignRefObject(IIO io, SqliteManager sql, IEventAggregator ea, ZmqManager zm)
|
|
|
{
|
|
|
this.iO = io as EzIO;
|
|
|
+ this.iO.OnChangedIO += IO_OnChangedIO;
|
|
|
+
|
|
|
this.sql = sql;
|
|
|
this.eventAggregator = ea;
|
|
|
- this.iO.OnChangedIO += IO_OnChangedIO;
|
|
|
+ this.zmqManager = zm;
|
|
|
+
|
|
|
+ this.threadCancel.AddGo(_DoControlSteer);
|
|
|
+ }
|
|
|
+
|
|
|
+ #region Thread Method
|
|
|
+ void _DoControlSteer()
|
|
|
+ {
|
|
|
+ while ( !this.threadCancel.Canceled )
|
|
|
+ {
|
|
|
+ var q = this.qReqSteer.Dequeue();
|
|
|
+ this.ControlSteerNWaitDone( q );
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ void ControlSteerNWaitDone( eSteeringState requestState )
|
|
|
+ {
|
|
|
+ logger.D( $"Steer - Start {requestState} -------------------------------------------------------" );
|
|
|
+
|
|
|
+ if ( requestState == eSteeringState.Left )
|
|
|
+ {
|
|
|
+ this.iO.WriteOutputIO( "OUT_F_STEERING_CWCCW", false );
|
|
|
+ this.iO.WriteOutputIO( "OUT_R_STEERING_CWCCW", true );
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ this.iO.WriteOutputIO( "OUT_F_STEERING_CWCCW", true );
|
|
|
+ this.iO.WriteOutputIO( "OUT_R_STEERING_CWCCW", false );
|
|
|
+ }
|
|
|
+
|
|
|
+ //Hight Voltage On
|
|
|
+ this.iO.WriteOutputIO( "OUT_F_STEERING_DA", true );
|
|
|
+ this.iO.WriteOutputIO( "OUT_R_STEERING_DA", true );
|
|
|
+
|
|
|
+ logger.D( $"Steer - OutPut On" );
|
|
|
+
|
|
|
+ var sT = SwUtils.CurrentTimeMillis;
|
|
|
+ bool isCheckedOutput = false;
|
|
|
+ var currentDirection = eSteeringState.None;
|
|
|
+ long eT = 0;
|
|
|
+ while ( true )
|
|
|
+ {
|
|
|
+ LockUtils.Wait( 10 );
|
|
|
+
|
|
|
+ currentDirection = GetSteerDirection();
|
|
|
+ if ( requestState == currentDirection )
|
|
|
+ break;
|
|
|
+
|
|
|
+ eT = SwUtils.Elapsed( sT );
|
|
|
+
|
|
|
+ if ( eT > 700 && !isCheckedOutput )
|
|
|
+ {
|
|
|
+ this.iO.WriteOutputIO( "OUT_F_STEERING_DA", false );
|
|
|
+ this.iO.WriteOutputIO( "OUT_R_STEERING_DA", false );
|
|
|
+ isCheckedOutput = true;
|
|
|
+ logger.D( $"Steer - Output Off {eT}ms" );
|
|
|
+ }
|
|
|
+
|
|
|
+ if ( eT > ConstUtils.ONE_SECOND * 2 )
|
|
|
+ {
|
|
|
+ logger.D( $"Steer - Wait Time Out {eT}ms" );
|
|
|
+ this.SteeringState = eSteeringState.None;
|
|
|
+
|
|
|
+ if ( !isCheckedOutput )
|
|
|
+ {
|
|
|
+ this.iO.WriteOutputIO( "OUT_F_STEERING_DA", false );
|
|
|
+ this.iO.WriteOutputIO( "OUT_R_STEERING_DA", false );
|
|
|
+ logger.D( $"Steer - Output Off {SwUtils.Elapsed( sT )}ms" );
|
|
|
+ }
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if ( !isCheckedOutput ) //위에서 Off 를 안했을 경우가 생길 수 있음. (700ms 이내에 조향 바뀌었을때)
|
|
|
+ {
|
|
|
+ this.iO.WriteOutputIO( "OUT_F_STEERING_DA", false );
|
|
|
+ this.iO.WriteOutputIO( "OUT_R_STEERING_DA", false );
|
|
|
+ logger.D( $"Steer - Output Off {SwUtils.Elapsed( sT )}ms" );
|
|
|
+ }
|
|
|
+
|
|
|
+ logger.D( $"Steer - Drive Request Start" );
|
|
|
+ this.SteeringState = currentDirection;
|
|
|
+ logger.D( $"Steer - Contorl End {SwUtils.Elapsed( sT )}ms -------------------------------------------------------" );
|
|
|
+
|
|
|
+ SteerLogger.I( SwUtils.Elapsed( sT ) );
|
|
|
+ }
|
|
|
+
|
|
|
+ eSteeringState GetSteerDirection()
|
|
|
+ {
|
|
|
+ if ( this.IsLeft() )
|
|
|
+ return eSteeringState.Left;
|
|
|
+ if ( this.IsRight() )
|
|
|
+ return eSteeringState.Right;
|
|
|
+
|
|
|
+ return eSteeringState.None;
|
|
|
+ }
|
|
|
+
|
|
|
+ public void RequestControl( eSteeringState state )
|
|
|
+ {
|
|
|
+ this.qReqSteer.Enqueue( state );
|
|
|
}
|
|
|
|
|
|
+ #endregion
|
|
|
+
|
|
|
private void IO_OnChangedIO(BitBlock bit)
|
|
|
{
|
|
|
//if ( bit.Tag.Equals( "IN_F_STEERING_DETECT_LEFT" ) || bit.Tag.Equals( "IN_F_STEERING_DETECT_RIGHT" ) )
|
|
|
@@ -66,7 +183,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
rearState = eSteeringState.None;
|
|
|
if (!this.IsRearLeft() && !this.IsRearRight())
|
|
|
rearState = eSteeringState.None;
|
|
|
-
|
|
|
+
|
|
|
if (this.IsRearRight())
|
|
|
rearState = eSteeringState.Right;
|
|
|
else if (this.IsRearLeft())
|
|
|
@@ -74,14 +191,15 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
else
|
|
|
rearState = eSteeringState.None;
|
|
|
|
|
|
+ eSteeringState totalState = eSteeringState.None;
|
|
|
if (frontState == eSteeringState.Left && rearState == eSteeringState.Left)
|
|
|
- this.SteeringState = eSteeringState.Left;
|
|
|
+ totalState = eSteeringState.Left;
|
|
|
else if (frontState == eSteeringState.Right && rearState == eSteeringState.Right)
|
|
|
- this.SteeringState = eSteeringState.Right;
|
|
|
+ totalState = eSteeringState.Right;
|
|
|
else
|
|
|
- this.SteeringState = eSteeringState.None;
|
|
|
+ totalState = eSteeringState.None;
|
|
|
|
|
|
- return this.SteeringState;
|
|
|
+ return totalState;
|
|
|
}
|
|
|
|
|
|
readonly object lockObject = new object();
|
|
|
@@ -151,7 +269,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
logger.D($"[Steer Release Lock] - {SwUtils.Elapsed(sT)} ms");
|
|
|
|
|
|
LockUtils.Wait(200);
|
|
|
- GetSteeringState();
|
|
|
+ this.SteeringState = GetSteeringState();
|
|
|
this.isExecuteSteering = false;
|
|
|
|
|
|
});
|
|
|
@@ -219,19 +337,100 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
return true;
|
|
|
}).ContinueWith(t =>
|
|
|
{
|
|
|
- logger.D($"[Steer Release Lock] - {SwUtils.Elapsed(sT)} ms");
|
|
|
+ var eT = SwUtils.Elapsed(sT);
|
|
|
+ logger.D($"[Steer Release Lock] - {eT} ms");
|
|
|
|
|
|
if (!t.Result)
|
|
|
this.SteeringState = eSteeringState.None;
|
|
|
else
|
|
|
{
|
|
|
LockUtils.Wait(200);
|
|
|
- GetSteeringState();
|
|
|
+ this.SteeringState = GetSteeringState();
|
|
|
}
|
|
|
|
|
|
this.isExecuteSteering = false;
|
|
|
});
|
|
|
}
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Test Control
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="isLeft"></param>
|
|
|
+ public void TestSteerControl(bool isLeft = false)
|
|
|
+ {
|
|
|
+ if (this.isExecuteSteering) return;
|
|
|
+
|
|
|
+ this.isExecuteSteering = true;
|
|
|
+
|
|
|
+ logger.D($"[Steer Lock] ----------------------------------------------------------------");
|
|
|
+
|
|
|
+ var sT = SwUtils.CurrentTimeMillis;
|
|
|
+
|
|
|
+ Task.Run(() =>
|
|
|
+ {
|
|
|
+ if (isLeft) //Left IO 가 직진 이라고 생각하자.
|
|
|
+ {
|
|
|
+ this.iO.WriteOutputIO("OUT_F_STEERING_CWCCW", false);
|
|
|
+ this.iO.WriteOutputIO("OUT_R_STEERING_CWCCW", true);
|
|
|
+
|
|
|
+ this.iO.WriteOutputIO("OUT_F_STEERING_DA", true);
|
|
|
+ this.iO.WriteOutputIO("OUT_R_STEERING_DA", true);
|
|
|
+ LockUtils.Wait(700);
|
|
|
+ this.iO.WriteOutputIO("OUT_F_STEERING_DA", false);
|
|
|
+ this.iO.WriteOutputIO("OUT_R_STEERING_DA", false);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ this.iO.WriteOutputIO("OUT_F_STEERING_CWCCW", true);
|
|
|
+ this.iO.WriteOutputIO("OUT_R_STEERING_CWCCW", false);
|
|
|
+
|
|
|
+ this.iO.WriteOutputIO("OUT_F_STEERING_DA", true);
|
|
|
+ this.iO.WriteOutputIO("OUT_R_STEERING_DA", true);
|
|
|
+ LockUtils.Wait(700);
|
|
|
+ this.iO.WriteOutputIO("OUT_F_STEERING_DA", false);
|
|
|
+ this.iO.WriteOutputIO("OUT_R_STEERING_DA", false);
|
|
|
+ }
|
|
|
+ });
|
|
|
+
|
|
|
+ Task.Run(() =>
|
|
|
+ {
|
|
|
+ long sTime = SwUtils.CurrentTimeMillis;
|
|
|
+ while (true)
|
|
|
+ {
|
|
|
+ LockUtils.Wait(5);
|
|
|
+
|
|
|
+ if (SwUtils.Gt(sTime, 2 * ConstUtils.ONE_SECOND))
|
|
|
+ {
|
|
|
+ this.SteeringState = eSteeringState.None;
|
|
|
+ this.isExecuteSteering = false;
|
|
|
+ logger.D($"Steer Control Fail {SwUtils.Elapsed(sT)}");
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (isLeft)
|
|
|
+ {
|
|
|
+ if (this.IsLeft())
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if (this.IsRight())
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ LockUtils.Wait(200); //안정화 시간.
|
|
|
+
|
|
|
+ logger.D($"Request Steer State Start");
|
|
|
+ this.SteeringState = GetSteeringState();
|
|
|
+ logger.D($"Request Steer State End");
|
|
|
+
|
|
|
+ var eT = SwUtils.Elapsed(sT);
|
|
|
+ logger.D($"[Steer Release Lock] - {eT} ms ----------------------------------------------------------");
|
|
|
+ SteerLogger.I(eT);
|
|
|
+ });
|
|
|
+
|
|
|
+ this.isExecuteSteering = false;
|
|
|
+ }
|
|
|
|
|
|
void SetHightVolt()
|
|
|
{
|