|
|
@@ -1,9 +1,9 @@
|
|
|
using System;
|
|
|
-using System.Security.Principal;
|
|
|
+using System.Collections.Generic;
|
|
|
+using System.Linq;
|
|
|
using System.Threading.Tasks;
|
|
|
using GSG.NET.Concurrent;
|
|
|
using GSG.NET.Logging;
|
|
|
-using GSG.NET.Quartz;
|
|
|
using GSG.NET.Utils;
|
|
|
using OHV.Common.Shareds;
|
|
|
using OHV.SqliteDAL;
|
|
|
@@ -19,7 +19,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
static Logger SteerLogger = Logger.GetLogger("SteerLogger");
|
|
|
|
|
|
ThreadCancel threadCancel = new ThreadCancel();
|
|
|
- TsQueue<eSteeringState> qReqSteer = new TsQueue<eSteeringState>();
|
|
|
+ TsQueue<eSteerCommand> qReqSteer = new TsQueue<eSteerCommand>();
|
|
|
|
|
|
EzIO iO = null;
|
|
|
SqliteManager sql = null;
|
|
|
@@ -58,100 +58,262 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
#region Thread Method
|
|
|
void _DoControlSteer()
|
|
|
{
|
|
|
- while ( !this.threadCancel.Canceled )
|
|
|
+ while (!this.threadCancel.Canceled)
|
|
|
{
|
|
|
var q = this.qReqSteer.Dequeue();
|
|
|
- this.ControlSteerNWaitDone( q );
|
|
|
+ //this.ControlSteerNWaitDone( q );
|
|
|
+ this.ExecuteCommandSteer(q);
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- void ControlSteerNWaitDone( eSteeringState requestState )
|
|
|
+ void ControlSteerNWaitDone(eSteeringState requestState)
|
|
|
{
|
|
|
- logger.D( $"Steer - Start {requestState} -------------------------------------------------------" );
|
|
|
+ logger.D($"Steer - Start {requestState} -------------------------------------------------------");
|
|
|
|
|
|
- if ( requestState == eSteeringState.Left )
|
|
|
+ if (requestState == eSteeringState.Left)
|
|
|
{
|
|
|
- this.iO.WriteOutputIO( "OUT_F_STEERING_CWCCW", false );
|
|
|
- this.iO.WriteOutputIO( "OUT_R_STEERING_CWCCW", true );
|
|
|
+ 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 );
|
|
|
+ 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 );
|
|
|
+ this.iO.WriteOutputIO("OUT_F_STEERING_DA", true);
|
|
|
+ this.iO.WriteOutputIO("OUT_R_STEERING_DA", true);
|
|
|
|
|
|
- logger.D( $"Steer - OutPut On" );
|
|
|
+ logger.D($"Steer - OutPut On");
|
|
|
|
|
|
var sT = SwUtils.CurrentTimeMillis;
|
|
|
bool isCheckedOutput = false;
|
|
|
var currentDirection = eSteeringState.None;
|
|
|
long eT = 0;
|
|
|
- while ( true )
|
|
|
+ while (true)
|
|
|
{
|
|
|
- LockUtils.Wait( 10 );
|
|
|
+ LockUtils.Wait(10);
|
|
|
|
|
|
currentDirection = GetSteerDirection();
|
|
|
- if ( requestState == currentDirection )
|
|
|
+ if (requestState == currentDirection)
|
|
|
break;
|
|
|
|
|
|
- eT = SwUtils.Elapsed( sT );
|
|
|
+ eT = SwUtils.Elapsed(sT);
|
|
|
|
|
|
- if ( eT > 700 && !isCheckedOutput )
|
|
|
+ if (eT > 700 && !isCheckedOutput)
|
|
|
{
|
|
|
- this.iO.WriteOutputIO( "OUT_F_STEERING_DA", false );
|
|
|
- this.iO.WriteOutputIO( "OUT_R_STEERING_DA", false );
|
|
|
+ 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" );
|
|
|
+ logger.D($"Steer - Output Off {eT}ms");
|
|
|
}
|
|
|
|
|
|
- if ( eT > ConstUtils.ONE_SECOND * 2 )
|
|
|
+ if (eT > ConstUtils.ONE_SECOND * 2)
|
|
|
{
|
|
|
- logger.D( $"Steer - Wait Time Out {eT}ms" );
|
|
|
+ logger.D($"Steer - Wait Time Out {eT}ms");
|
|
|
this.SteeringState = eSteeringState.None;
|
|
|
|
|
|
- if ( !isCheckedOutput )
|
|
|
+ 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" );
|
|
|
+ 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 이내에 조향 바뀌었을때)
|
|
|
+ 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" );
|
|
|
+ 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" );
|
|
|
+ logger.D($"Steer - Drive Request Start");
|
|
|
this.SteeringState = currentDirection;
|
|
|
- logger.D( $"Steer - Contorl End {SwUtils.Elapsed( sT )}ms -------------------------------------------------------" );
|
|
|
+ logger.D($"Steer - Contorl End {SwUtils.Elapsed(sT)}ms -------------------------------------------------------");
|
|
|
|
|
|
- SteerLogger.I( SwUtils.Elapsed( sT ) );
|
|
|
+ SteerLogger.I(SwUtils.Elapsed(sT));
|
|
|
+ }
|
|
|
+
|
|
|
+ void ExecuteCommandSteer(eSteerCommand command)
|
|
|
+ {
|
|
|
+ logger.D($"Execute Steer Command - {command}");
|
|
|
+ var cmdSt = SwUtils.CurrentTimeMillis;
|
|
|
+
|
|
|
+ Action<bool> actionSteerFront = (isLeft) =>
|
|
|
+ {
|
|
|
+ if (isLeft)
|
|
|
+ this.iO.WriteOutputIO("OUT_F_STEERING_CWCCW", false);
|
|
|
+ else
|
|
|
+ this.iO.WriteOutputIO("OUT_F_STEERING_CWCCW", true);
|
|
|
+ };
|
|
|
+
|
|
|
+ Action<bool> actionSteerRear = (isLeft) =>
|
|
|
+ {
|
|
|
+ if (isLeft)
|
|
|
+ this.iO.WriteOutputIO("OUT_R_STEERING_CWCCW", true);
|
|
|
+ else
|
|
|
+ this.iO.WriteOutputIO("OUT_R_STEERING_CWCCW", false);
|
|
|
+ };
|
|
|
+
|
|
|
+ Action<bool> actionOnFrontHightVolteOn = (isOn) =>
|
|
|
+ {
|
|
|
+ if (isOn)
|
|
|
+ this.iO.WriteOutputIO("OUT_F_STEERING_DA", true);
|
|
|
+ else
|
|
|
+ this.iO.WriteOutputIO("OUT_F_STEERING_DA", false);
|
|
|
+ };
|
|
|
+
|
|
|
+ Action<bool> actionOnRearHightVolteOn = (isOn) =>
|
|
|
+ {
|
|
|
+ if (isOn)
|
|
|
+ this.iO.WriteOutputIO("OUT_R_STEERING_DA", true);
|
|
|
+ else
|
|
|
+ this.iO.WriteOutputIO("OUT_R_STEERING_DA", false);
|
|
|
+ };
|
|
|
+
|
|
|
+ Action<eSteerCommand> actionSteer = (cmd) =>
|
|
|
+ {
|
|
|
+ List<Func<bool>> inputList = new List<Func<bool>>();
|
|
|
+ List<Action<bool>> outputList = new List<Action<bool>>();
|
|
|
+
|
|
|
+ switch (cmd)
|
|
|
+ {
|
|
|
+ case eSteerCommand.LL:
|
|
|
+ inputList.Add(IsFrontLeft);
|
|
|
+ inputList.Add(IsRearLeft);
|
|
|
+ actionSteerFront(true);
|
|
|
+ actionSteerRear(true);
|
|
|
+ outputList.Add(actionOnRearHightVolteOn);
|
|
|
+ outputList.Add(actionOnFrontHightVolteOn);
|
|
|
+ break;
|
|
|
+ case eSteerCommand.LX:
|
|
|
+ inputList.Add(IsFrontLeft);
|
|
|
+ actionSteerFront(true);
|
|
|
+ outputList.Add(actionOnFrontHightVolteOn);
|
|
|
+ break;
|
|
|
+ case eSteerCommand.XL:
|
|
|
+ inputList.Add(IsRearLeft);
|
|
|
+ actionSteerRear(true);
|
|
|
+ outputList.Add(actionOnRearHightVolteOn);
|
|
|
+ break;
|
|
|
+ case eSteerCommand.RR:
|
|
|
+ inputList.Add(IsFrontRight);
|
|
|
+ inputList.Add(IsRearRight);
|
|
|
+ actionSteerFront(false);
|
|
|
+ actionSteerRear(false);
|
|
|
+ outputList.Add(actionOnRearHightVolteOn);
|
|
|
+ outputList.Add(actionOnFrontHightVolteOn);
|
|
|
+ break;
|
|
|
+ case eSteerCommand.RX:
|
|
|
+ inputList.Add(IsFrontRight);
|
|
|
+ actionSteerFront(false);
|
|
|
+ outputList.Add(actionOnFrontHightVolteOn);
|
|
|
+ break;
|
|
|
+ case eSteerCommand.XR:
|
|
|
+ inputList.Add(IsRearRight);
|
|
|
+ actionSteerRear(false);
|
|
|
+ outputList.Add(actionOnRearHightVolteOn);
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ outputList.ForEach(x => x.Invoke(true));
|
|
|
+
|
|
|
+ var sT = SwUtils.CurrentTimeMillis;
|
|
|
+ while (true)
|
|
|
+ {
|
|
|
+ LockUtils.Wait(10);
|
|
|
+
|
|
|
+ if (inputList.All(x => x.Invoke()))
|
|
|
+ {
|
|
|
+ logger.D($"Execute Steer - {cmd} - Sensor On {SwUtils.Elapsed(sT)}mm");
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (SwUtils.Elapsed(sT) > 700)
|
|
|
+ {
|
|
|
+ logger.D($"Execute Steer - {cmd} - {SwUtils.Elapsed(sT)}mm");
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ outputList.ForEach(x => x.Invoke(false));
|
|
|
+ };
|
|
|
+
|
|
|
+ switch (command)
|
|
|
+ {
|
|
|
+ case eSteerCommand.LL:
|
|
|
+ case eSteerCommand.LX:
|
|
|
+ case eSteerCommand.XL:
|
|
|
+ case eSteerCommand.RR:
|
|
|
+ case eSteerCommand.RX:
|
|
|
+ case eSteerCommand.XR:
|
|
|
+ actionSteer(command);
|
|
|
+ break;
|
|
|
+ case eSteerCommand.OffOff:
|
|
|
+ //Todo: 전후방 전원 Off
|
|
|
+ this.iO.WriteOutputIO("OUT_F_STEERING_OFF", false);
|
|
|
+ this.iO.WriteOutputIO("OUT_R_STEERING_OFF", false);
|
|
|
+ break;
|
|
|
+ case eSteerCommand.OnOn:
|
|
|
+ this.iO.WriteOutputIO("OUT_F_STEERING_OFF", true);
|
|
|
+ this.iO.WriteOutputIO("OUT_R_STEERING_OFF", true);
|
|
|
+ break;
|
|
|
+ case eSteerCommand.FrontOn:
|
|
|
+ this.iO.WriteOutputIO("OUT_F_STEERING_OFF", true);
|
|
|
+ break;
|
|
|
+ case eSteerCommand.RearOn:
|
|
|
+ this.iO.WriteOutputIO("OUT_R_STEERING_OFF", true);
|
|
|
+ break;
|
|
|
+ case eSteerCommand.FrontOff:
|
|
|
+ this.iO.WriteOutputIO("OUT_F_STEERING_OFF", false);
|
|
|
+ break;
|
|
|
+ case eSteerCommand.RearOff:
|
|
|
+ this.iO.WriteOutputIO("OUT_R_STEERING_OFF", false);
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ logger.D($"Complete Steer Command - {command} / {SwUtils.Elapsed(cmdSt)}ms");
|
|
|
}
|
|
|
|
|
|
eSteeringState GetSteerDirection()
|
|
|
{
|
|
|
- if ( this.IsLeft() )
|
|
|
+ if (this.IsLeft())
|
|
|
return eSteeringState.Left;
|
|
|
- if ( this.IsRight() )
|
|
|
+ if (this.IsRight())
|
|
|
return eSteeringState.Right;
|
|
|
|
|
|
return eSteeringState.None;
|
|
|
}
|
|
|
|
|
|
- public void RequestControl( eSteeringState state )
|
|
|
+ public void RequestControl(eSteeringState state)
|
|
|
{
|
|
|
- this.qReqSteer.Enqueue( state );
|
|
|
+ //this.qReqSteer.Enqueue( state );
|
|
|
+ switch (state)
|
|
|
+ {
|
|
|
+ case eSteeringState.None:
|
|
|
+ break;
|
|
|
+ case eSteeringState.Left:
|
|
|
+ this.RequestCommand(eSteerCommand.LL);
|
|
|
+ break;
|
|
|
+ case eSteeringState.Right:
|
|
|
+ this.RequestCommand(eSteerCommand.RR);
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
+ public void RequestCommand(eSteerCommand command) => this.qReqSteer.Enqueue(command);
|
|
|
+
|
|
|
+
|
|
|
#endregion
|
|
|
|
|
|
private void IO_OnChangedIO(BitBlock bit)
|
|
|
@@ -351,7 +513,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
this.isExecuteSteering = false;
|
|
|
});
|
|
|
}
|
|
|
-
|
|
|
+
|
|
|
/// <summary>
|
|
|
/// Test Control
|
|
|
/// </summary>
|