|
|
@@ -6,6 +6,7 @@ using System.Threading;
|
|
|
using System.Threading.Tasks;
|
|
|
using FluentResults;
|
|
|
using GSG.NET.Concurrent;
|
|
|
+using GSG.NET.Extensions;
|
|
|
using GSG.NET.LINQ;
|
|
|
using GSG.NET.Logging;
|
|
|
using GSG.NET.Quartz;
|
|
|
@@ -42,7 +43,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
}
|
|
|
|
|
|
static Logger logger = Logger.GetLogger();
|
|
|
- static Logger loggerPIO = Logger.GetLogger( "PIO" );
|
|
|
+ static Logger loggerPIO = Logger.GetLogger("PIO");
|
|
|
|
|
|
#region Properties
|
|
|
private double currentPosition;
|
|
|
@@ -50,18 +51,39 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
/// <summary>
|
|
|
/// Tag 위치
|
|
|
/// </summary>
|
|
|
+ private int currentTag;
|
|
|
+
|
|
|
+ public int CurrentTag
|
|
|
+ {
|
|
|
+ get { return currentTag; }
|
|
|
+ set { SetField(ref this.currentTag, value); }
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// Scale Value
|
|
|
+ /// </summary>
|
|
|
public double CurrentPosition
|
|
|
{
|
|
|
get { return currentPosition; }
|
|
|
set
|
|
|
{
|
|
|
- if ( this.currentPosition == value ) return;
|
|
|
-
|
|
|
- this.currentPosition = value;
|
|
|
- this.OnCurrentPotisionChanged?.Invoke( (int)value );
|
|
|
+ if (SetField(ref this.currentPosition, value))
|
|
|
+ {
|
|
|
+ this.currentPosition = value;
|
|
|
+ this.OnCurrentPotisionChanged?.Invoke((int)value);
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+ private eSteeringState steeringState;
|
|
|
+
|
|
|
+ public eSteeringState SteeringState
|
|
|
+ {
|
|
|
+ get { return steeringState; }
|
|
|
+ set { SetField(ref this.steeringState, value); }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
//이동
|
|
|
public bool Busy
|
|
|
{
|
|
|
@@ -69,9 +91,9 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
|
return this.CurrentSubCommand == null ? false : true;
|
|
|
}
|
|
|
-
|
|
|
- private set { }
|
|
|
+ set { }
|
|
|
}
|
|
|
+
|
|
|
public bool IsMoving { get; set; }
|
|
|
public double BatteryVolt { get; set; }
|
|
|
public bool IsError { get; set; }
|
|
|
@@ -127,36 +149,37 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
public eObstacleState ObstacleStateProperty
|
|
|
{
|
|
|
get { return obstacleState; }
|
|
|
- set { SetField( ref this.obstacleState, value ); }
|
|
|
+ set { SetField(ref this.obstacleState, value); }
|
|
|
}
|
|
|
|
|
|
private eVehicleState vehicleState;
|
|
|
public eVehicleState VehicleStateProperty
|
|
|
{
|
|
|
get { return vehicleState; }
|
|
|
- set { SetField( ref this.vehicleState, value ); }
|
|
|
+ set { SetField(ref this.vehicleState, value); }
|
|
|
}
|
|
|
|
|
|
IEventAggregator eventAggregator;
|
|
|
|
|
|
- public Vehicle( IIO io, SqliteManager sqliteManager, IEventAggregator ea, AutoManager auto )
|
|
|
+ public Vehicle(IIO io, SqliteManager sqliteManager, IEventAggregator ea, AutoManager auto)
|
|
|
{
|
|
|
this.iO = io;
|
|
|
this.motion = new GSIMotion();
|
|
|
this.sql = sqliteManager;
|
|
|
this.autoManager = auto;
|
|
|
|
|
|
- this.obstacleBitList.AddRange( new string[]
|
|
|
+ this.obstacleBitList.AddRange(new string[]
|
|
|
{
|
|
|
"OUT_OBSTRUCTION_PATTERN_00",
|
|
|
"OUT_OBSTRUCTION_PATTERN_01",
|
|
|
"OUT_OBSTRUCTION_PATTERN_02",
|
|
|
"OUT_OBSTRUCTION_PATTERN_03",
|
|
|
"OUT_OBSTRUCTION_PATTERN_04",
|
|
|
- } );
|
|
|
+ });
|
|
|
|
|
|
this.eventAggregator = ea;
|
|
|
|
|
|
+<<<<<<< HEAD
|
|
|
/*Drive*/
|
|
|
this.eventAggregator.GetEvent<DriveControlPubSubEvent>().Unsubscribe( ReceiveDriveControlEvent );
|
|
|
this.eventAggregator.GetEvent<DriveControlPubSubEvent>().Subscribe( ReceiveDriveControlEvent );
|
|
|
@@ -186,17 +209,21 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
break;
|
|
|
}
|
|
|
}
|
|
|
+=======
|
|
|
+ this.eventAggregator.GetEvent<DriveControlPubSubEvent>().Unsubscribe(ReceiveDriveControlEvent);
|
|
|
+ this.eventAggregator.GetEvent<DriveControlPubSubEvent>().Subscribe(ReceiveDriveControlEvent);
|
|
|
+>>>>>>> f2b1b12e8c9f7450e1cbe9e6d15261074bce9945
|
|
|
}
|
|
|
|
|
|
- private void ReceiveDriveControlEvent( DriveControlEventArgs _args )
|
|
|
+ private void ReceiveDriveControlEvent(DriveControlEventArgs _args)
|
|
|
{
|
|
|
- if ( this.autoManager.OperationModeProperty != eOperatationMode.ManualMode )
|
|
|
+ if (this.autoManager.OperationModeProperty != eOperatationMode.ManualMode)
|
|
|
return;
|
|
|
|
|
|
var msg = _args;
|
|
|
- if ( msg.EventDir == DriveControlEventArgs.eEventDir.ToBack )
|
|
|
+ if (msg.EventDir == DriveControlEventArgs.eEventDir.ToBack)
|
|
|
{
|
|
|
- switch ( msg.ControlKind )
|
|
|
+ switch (msg.ControlKind)
|
|
|
{
|
|
|
case DriveControlEventArgs.eControlKind.MOVE:
|
|
|
this.ReqMoveToPos(_args);
|
|
|
@@ -204,8 +231,8 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
case DriveControlEventArgs.eControlKind.STOP:
|
|
|
break;
|
|
|
case DriveControlEventArgs.eControlKind.Steering:
|
|
|
- if ( msg.MoveDir == DriveControlEventArgs.eMoveDir.LEFT )
|
|
|
- this.steering.ControlSteering( true );
|
|
|
+ if (msg.MoveDir == DriveControlEventArgs.eMoveDir.LEFT)
|
|
|
+ this.steering.ControlSteering(true);
|
|
|
else
|
|
|
this.steering.ControlSteering();
|
|
|
break;
|
|
|
@@ -213,11 +240,11 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
|
DriveControlEventArgs reply = new DriveControlEventArgs();
|
|
|
reply.ControlKind = DriveControlEventArgs.eControlKind.SteeringState;
|
|
|
- if ( this.steering.IsLeft() )
|
|
|
- reply.Result = FluentResults.Results.Ok<DriveControlEventArgs.eMoveDir>( DriveControlEventArgs.eMoveDir.LEFT );
|
|
|
+ if (this.steering.IsLeft())
|
|
|
+ reply.Result = FluentResults.Results.Ok<DriveControlEventArgs.eMoveDir>(DriveControlEventArgs.eMoveDir.LEFT);
|
|
|
else
|
|
|
- reply.Result = FluentResults.Results.Ok<DriveControlEventArgs.eMoveDir>( DriveControlEventArgs.eMoveDir.RIGHT );
|
|
|
- this.DriveControlEventPublish( reply );
|
|
|
+ reply.Result = FluentResults.Results.Ok<DriveControlEventArgs.eMoveDir>(DriveControlEventArgs.eMoveDir.RIGHT);
|
|
|
+ this.DriveControlEventPublish(reply);
|
|
|
}
|
|
|
break;
|
|
|
case DriveControlEventArgs.eControlKind.ReqCurrentPos:
|
|
|
@@ -228,16 +255,16 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
this.taskCancel.WaitAll();
|
|
|
break;
|
|
|
case DriveControlEventArgs.eControlKind.FaultReset:
|
|
|
- this.ReqFaultReset( _args );
|
|
|
+ this.ReqFaultReset(_args);
|
|
|
break;
|
|
|
case DriveControlEventArgs.eControlKind.DriveON:
|
|
|
- this.ReqDriveOn( _args );
|
|
|
+ this.ReqDriveOn(_args);
|
|
|
break;
|
|
|
case DriveControlEventArgs.eControlKind.DriveOFF:
|
|
|
- this.ReqDriveOff( _args );
|
|
|
+ this.ReqDriveOff(_args);
|
|
|
break;
|
|
|
case DriveControlEventArgs.eControlKind.JOG:
|
|
|
- this.ReqJog( _args );
|
|
|
+ this.ReqJog(_args);
|
|
|
break;
|
|
|
default:
|
|
|
break;
|
|
|
@@ -245,11 +272,15 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+<<<<<<< HEAD
|
|
|
|
|
|
private void DriveControlEventPublish( DriveControlEventArgs args )
|
|
|
+=======
|
|
|
+ private void DriveControlEventPublish(DriveControlEventArgs args)
|
|
|
+>>>>>>> f2b1b12e8c9f7450e1cbe9e6d15261074bce9945
|
|
|
{
|
|
|
args.EventDir = DriveControlEventArgs.eEventDir.ToFront;
|
|
|
- this.eventAggregator.GetEvent<DriveControlPubSubEvent>().Publish( args );
|
|
|
+ this.eventAggregator.GetEvent<DriveControlPubSubEvent>().Publish(args);
|
|
|
}
|
|
|
|
|
|
public void Init()
|
|
|
@@ -258,17 +289,19 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
this.CreateSteering();
|
|
|
|
|
|
ThreadStart();
|
|
|
+
|
|
|
+ //TimerUtils.Once(5000, () => { this.CurrentPosition = 1000; });
|
|
|
}
|
|
|
|
|
|
public int InitializationVehicle()
|
|
|
{
|
|
|
int result = 0;
|
|
|
- if ( this.IsDetectedCenter() ) //자제가 있으면 Lock
|
|
|
+ if (this.IsDetectedCenter()) //자제가 있으면 Lock
|
|
|
result = this.clamp.Lock_Sync();
|
|
|
else
|
|
|
result = this.clamp.Unlock_Sync();
|
|
|
|
|
|
- if ( this.motion.IsErrorOn )
|
|
|
+ if (this.motion.IsErrorOn)
|
|
|
return 22;
|
|
|
|
|
|
return result;
|
|
|
@@ -308,16 +341,16 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
ControlKind = DriveControlEventArgs.eControlKind.FaultReset
|
|
|
};
|
|
|
|
|
|
- msg.Result = Results.Ok( "Drive On" );
|
|
|
- this.DriveControlEventPublish( msg );
|
|
|
+ msg.Result = Results.Ok("Drive On");
|
|
|
+ this.DriveControlEventPublish(msg);
|
|
|
}
|
|
|
|
|
|
void ReqJog(DriveControlEventArgs _args)
|
|
|
{
|
|
|
//TODO:[20/03/18 ys-hwang] Drive Jog Request
|
|
|
var drive = string.Empty;
|
|
|
-
|
|
|
- if ( _args.JogDir == DriveControlEventArgs.eJogMoveDir.Positive )
|
|
|
+
|
|
|
+ if (_args.JogDir == DriveControlEventArgs.eJogMoveDir.Positive)
|
|
|
drive = "POSITIVE";
|
|
|
else
|
|
|
drive = "NEGATIVE";
|
|
|
@@ -327,12 +360,13 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
|
//TODO:[20/03/18 ys-hwang] Drive Current Position Publish
|
|
|
|
|
|
- var task = Task.Factory.StartNew( ( ) =>
|
|
|
- {
|
|
|
- while(!this.taskCancel.Canceled)
|
|
|
- {
|
|
|
- LockUtils.Wait( 500 );
|
|
|
+ var task = Task.Factory.StartNew(() =>
|
|
|
+ {
|
|
|
+ while (!this.taskCancel.Canceled)
|
|
|
+ {
|
|
|
+ LockUtils.Wait(500);
|
|
|
|
|
|
+<<<<<<< HEAD
|
|
|
var msg = new DriveControlEventArgs
|
|
|
{
|
|
|
EventDir = DriveControlEventArgs.eEventDir.ToFront ,
|
|
|
@@ -341,13 +375,23 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
};
|
|
|
|
|
|
this.DriveControlEventPublish( msg );
|
|
|
+=======
|
|
|
+ var msg = new DriveControlEventArgs
|
|
|
+ {
|
|
|
+ EventDir = DriveControlEventArgs.eEventDir.ToFront,
|
|
|
+ ControlKind = DriveControlEventArgs.eControlKind.ReqCurrentPos,
|
|
|
+ //CurrentPosition = drive.CurrentPosition,
|
|
|
+ };
|
|
|
+
|
|
|
+ //this.DriveControlEventPublish( msg );
|
|
|
+>>>>>>> f2b1b12e8c9f7450e1cbe9e6d15261074bce9945
|
|
|
}
|
|
|
- } );
|
|
|
+ });
|
|
|
|
|
|
- this.taskCancel.Add( task );
|
|
|
+ this.taskCancel.Add(task);
|
|
|
}
|
|
|
|
|
|
- void ReqDriveOn( DriveControlEventArgs _args)
|
|
|
+ void ReqDriveOn(DriveControlEventArgs _args)
|
|
|
{
|
|
|
var drive = "Drive Name";
|
|
|
//drive.On();
|
|
|
@@ -356,57 +400,61 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
|
ControlKind = DriveControlEventArgs.eControlKind.DriveON
|
|
|
};
|
|
|
- msg.Result = Results.Ok( "Drive On" );
|
|
|
+ msg.Result = Results.Ok("Drive On");
|
|
|
|
|
|
- this.DriveControlEventPublish( msg );
|
|
|
+ this.DriveControlEventPublish(msg);
|
|
|
}
|
|
|
|
|
|
- void ReqDriveOff( DriveControlEventArgs _args)
|
|
|
+ void ReqDriveOff(DriveControlEventArgs _args)
|
|
|
{
|
|
|
var drive = "Drive Name";
|
|
|
//drive.Off();
|
|
|
|
|
|
var msg = new DriveControlEventArgs
|
|
|
{
|
|
|
+<<<<<<< HEAD
|
|
|
ControlKind = DriveControlEventArgs.eControlKind.DriveOFF
|
|
|
+=======
|
|
|
+
|
|
|
+>>>>>>> f2b1b12e8c9f7450e1cbe9e6d15261074bce9945
|
|
|
};
|
|
|
- msg.Result = Results.Ok( "Drive On" );
|
|
|
+ msg.Result = Results.Ok("Drive On");
|
|
|
|
|
|
- this.DriveControlEventPublish( msg );
|
|
|
+ this.DriveControlEventPublish(msg);
|
|
|
}
|
|
|
#endregion
|
|
|
|
|
|
#region Thread
|
|
|
void ThreadStart()
|
|
|
{
|
|
|
- this.cancel.AddGo( new Action( this._ThSubCmdWorker ) );
|
|
|
- this.cancel.AddGo( new Action( this._ThObstacleChecker ) );
|
|
|
+ this.cancel.AddGo(new Action(this._ThSubCmdWorker));
|
|
|
+ this.cancel.AddGo(new Action(this._ThObstacleChecker));
|
|
|
}
|
|
|
|
|
|
//장애물 감지 Thread
|
|
|
//장애물 감지 패턴 변경도 여기 하자.
|
|
|
private void _ThObstacleChecker()
|
|
|
{
|
|
|
- while ( !this.cancel.Canceled )
|
|
|
+ while (!this.cancel.Canceled)
|
|
|
{
|
|
|
try
|
|
|
{
|
|
|
- if ( this.autoManager.OperationModeProperty == eOperatationMode.AutoMode )
|
|
|
+ if (this.autoManager.OperationModeProperty == eOperatationMode.AutoMode)
|
|
|
this.CheckObstacle();
|
|
|
}
|
|
|
- catch ( ThreadInterruptedException threadInterruptedException )
|
|
|
+ catch (ThreadInterruptedException threadInterruptedException)
|
|
|
{
|
|
|
}
|
|
|
- catch ( Exception exception )
|
|
|
+ catch (Exception exception)
|
|
|
{
|
|
|
- logger.E( exception );
|
|
|
+ logger.E(exception);
|
|
|
}
|
|
|
finally
|
|
|
{
|
|
|
- LockUtils.Wait( 5 );
|
|
|
+ LockUtils.Wait(5);
|
|
|
}
|
|
|
}
|
|
|
- logger.D( "Vehicle - _ThObstacleChecker Dispose" );
|
|
|
+ logger.D("Vehicle - _ThObstacleChecker Dispose");
|
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
|
@@ -414,66 +462,66 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
/// </summary>
|
|
|
public void _ThSubCmdWorker()
|
|
|
{
|
|
|
- while ( !this.cancel.Canceled )
|
|
|
+ while (!this.cancel.Canceled)
|
|
|
{
|
|
|
try
|
|
|
{
|
|
|
- if ( this.ObstacleStateProperty != eObstacleState.Normal ) //장애물 감지 상태 시 조그 동작만 가능하게.
|
|
|
+ if (this.ObstacleStateProperty != eObstacleState.Normal) //장애물 감지 상태 시 조그 동작만 가능하게.
|
|
|
continue;
|
|
|
|
|
|
- if ( this.autoManager.AutoModeStateProperty != eAutoModeState.Run ) //
|
|
|
+ if (this.autoManager.AutoModeStateProperty != eAutoModeState.Run) //
|
|
|
continue;
|
|
|
|
|
|
var subCmd = sql.SubCmdDAL.GetSubCmd();
|
|
|
- if ( subCmd == null ) continue;
|
|
|
+ if (subCmd == null) continue;
|
|
|
|
|
|
- if ( !sql.CommandDAL.All.Any( x => x.CommandID.Equals( subCmd.CmdID ) ) )
|
|
|
+ if (!sql.CommandDAL.All.Any(x => x.CommandID.Equals(subCmd.CmdID)))
|
|
|
{
|
|
|
- if ( subCmd.CmdType == SubCmd.eCmdType.Auto ) //자동 명령중 Main Command 가 없으면 삭제.
|
|
|
+ if (subCmd.CmdType == SubCmd.eCmdType.Auto) //자동 명령중 Main Command 가 없으면 삭제.
|
|
|
{
|
|
|
- sql.SubCmdDAL.Delete( subCmd );
|
|
|
- logger.I( $"SubCmd Deleted - ID={subCmd.ID}, CommandID={subCmd.CmdID}" );
|
|
|
+ sql.SubCmdDAL.Delete(subCmd);
|
|
|
+ logger.I($"SubCmd Deleted - ID={subCmd.ID}, CommandID={subCmd.CmdID}");
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- switch ( subCmd.Type )
|
|
|
+ switch (subCmd.Type)
|
|
|
{
|
|
|
case SubCmd.eType.Move:
|
|
|
this.CurrentSubCommand = subCmd;
|
|
|
- this.Move( subCmd );
|
|
|
+ this.Move(subCmd);
|
|
|
break;
|
|
|
|
|
|
case SubCmd.eType.Load:
|
|
|
this.CurrentSubCommand = subCmd;
|
|
|
- this.LoadCarrier( subCmd );
|
|
|
+ this.LoadCarrier(subCmd);
|
|
|
break;
|
|
|
|
|
|
case SubCmd.eType.Unload:
|
|
|
this.CurrentSubCommand = subCmd;
|
|
|
- this.UnloadCarrier( subCmd );
|
|
|
+ this.UnloadCarrier(subCmd);
|
|
|
break;
|
|
|
|
|
|
case SubCmd.eType.Charge:
|
|
|
this.CurrentSubCommand = subCmd;
|
|
|
- this.BatteryCharge( subCmd );
|
|
|
+ this.BatteryCharge(subCmd);
|
|
|
break;
|
|
|
default:
|
|
|
break;
|
|
|
}
|
|
|
}
|
|
|
- catch ( ThreadInterruptedException threadInterruptedException )
|
|
|
+ catch (ThreadInterruptedException threadInterruptedException)
|
|
|
{
|
|
|
}
|
|
|
- catch ( Exception exception )
|
|
|
+ catch (Exception exception)
|
|
|
{
|
|
|
- logger.E( exception );
|
|
|
+ logger.E(exception);
|
|
|
}
|
|
|
finally
|
|
|
{
|
|
|
- LockUtils.Wait( 500 );
|
|
|
+ LockUtils.Wait(500);
|
|
|
}
|
|
|
}
|
|
|
- logger.D( "Vehicle - _ThSubCmdWorker Dispose" );
|
|
|
+ logger.D("Vehicle - _ThSubCmdWorker Dispose");
|
|
|
}
|
|
|
#endregion
|
|
|
|
|
|
@@ -487,21 +535,21 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
this.autoManager.ProcessAlarm(23);
|
|
|
}
|
|
|
|
|
|
- void Move( SubCmd sub )
|
|
|
+ void Move(SubCmd sub)
|
|
|
{
|
|
|
- if ( this.MoveTo( sub.TargetID ) )
|
|
|
+ if (this.MoveTo(sub.TargetID))
|
|
|
{
|
|
|
- sql.SubCmdDAL.Delete( sub );
|
|
|
+ sql.SubCmdDAL.Delete(sub);
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- if ( this.ObstacleStateProperty == eObstacleState.Blocked )
|
|
|
+ if (this.ObstacleStateProperty == eObstacleState.Blocked)
|
|
|
{
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- bool MoveTo( string pointID )
|
|
|
+ bool MoveTo(string pointID)
|
|
|
{
|
|
|
//this.BuzzerOnOff(true, eBuzzerKind.StartWarn);
|
|
|
////TimerUtils.Once(3000, BuzzerOnOff, false, eBuzzerKind.StartWarn );
|
|
|
@@ -510,14 +558,14 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
|
|
|
this.OnMoveReady?.Invoke();
|
|
|
|
|
|
- var moveReadyBuzzerTime = sql.ConfigDal.GetValueToInt( ConstString.BuzzerStartReadyTime );
|
|
|
- Thread.Sleep( moveReadyBuzzerTime );
|
|
|
+ var moveReadyBuzzerTime = sql.ConfigDal.GetValueToInt(ConstString.BuzzerStartReadyTime);
|
|
|
+ Thread.Sleep(moveReadyBuzzerTime);
|
|
|
|
|
|
this.OnMoving?.Invoke();
|
|
|
this.IsMoving = true;
|
|
|
|
|
|
//this.BuzzerOnOff(true, eBuzzerKind.Moving);
|
|
|
- this.motion.MoveToPoint( pointID, 100 );
|
|
|
+ this.motion.MoveToPoint(pointID, 100);
|
|
|
|
|
|
bool result = Wait4MoveDone();
|
|
|
this.IsMoving = false;
|
|
|
@@ -533,17 +581,17 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
long st = SwUtils.CurrentTimeMillis;
|
|
|
|
|
|
//Todo: 이동시 확인 사항들.
|
|
|
- while ( true )
|
|
|
+ while (true)
|
|
|
{
|
|
|
- Thread.Sleep( 5 );
|
|
|
+ Thread.Sleep(5);
|
|
|
|
|
|
- if ( SwUtils.Gt( st, waitTime ) )
|
|
|
+ if (SwUtils.Gt(st, waitTime))
|
|
|
{
|
|
|
//Todo: 이동시간 초과 시 동작들.
|
|
|
break;
|
|
|
}
|
|
|
|
|
|
- if ( this.ObstacleStateProperty == eObstacleState.Blocked )
|
|
|
+ if (this.ObstacleStateProperty == eObstacleState.Blocked)
|
|
|
return false;
|
|
|
|
|
|
//Todo: 이동중 명령이 삭제 되면 처리 할일들.
|
|
|
@@ -555,70 +603,70 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
- public bool LoadCarrier( SubCmd sub )
|
|
|
+ public bool LoadCarrier(SubCmd sub)
|
|
|
{
|
|
|
- var route = sql.RouteDal.GetRoute( sub.TargetID );
|
|
|
+ var route = sql.RouteDal.GetRoute(sub.TargetID);
|
|
|
|
|
|
- if ( !CorrectPosition( route, this.CurrentPosition ) )
|
|
|
+ if (!CorrectPosition(route, this.CurrentPosition))
|
|
|
{
|
|
|
- this.autoManager.ProcessAlarm( 20 );
|
|
|
+ this.autoManager.ProcessAlarm(20);
|
|
|
return false; //Alarm
|
|
|
}
|
|
|
|
|
|
int result = this.clamp.Unlock_Sync();
|
|
|
- if ( result != 0 )
|
|
|
+ if (result != 0)
|
|
|
{
|
|
|
- this.autoManager.ProcessAlarm( result );
|
|
|
+ this.autoManager.ProcessAlarm(result);
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
- result = this.PIOAndLoad( sub.TargetID );
|
|
|
- if ( result != 0 )
|
|
|
+ result = this.PIOAndLoad(sub.TargetID);
|
|
|
+ if (result != 0)
|
|
|
{
|
|
|
- this.autoManager.ProcessAlarm( result );
|
|
|
+ this.autoManager.ProcessAlarm(result);
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
result = this.clamp.Lock_Sync();
|
|
|
- if ( result != 0 )
|
|
|
+ if (result != 0)
|
|
|
{
|
|
|
- this.autoManager.ProcessAlarm( result );
|
|
|
+ this.autoManager.ProcessAlarm(result);
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
//Load, Unload 가 끝나면 메인 Command 를 완료 했다고 판다.
|
|
|
- sql.CommandDAL.UpdateState( sub.CmdID, eCommandState.Complete );
|
|
|
- sql.SubCmdDAL.Delete( sub );
|
|
|
+ sql.CommandDAL.UpdateState(sub.CmdID, eCommandState.Complete);
|
|
|
+ sql.SubCmdDAL.Delete(sub);
|
|
|
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
- public bool UnloadCarrier( SubCmd sub )
|
|
|
+ public bool UnloadCarrier(SubCmd sub)
|
|
|
{
|
|
|
- var route = sql.RouteDal.GetRoute( sub.TargetID );
|
|
|
+ var route = sql.RouteDal.GetRoute(sub.TargetID);
|
|
|
|
|
|
- if ( !CorrectPosition( route, this.CurrentPosition ) )
|
|
|
+ if (!CorrectPosition(route, this.CurrentPosition))
|
|
|
{
|
|
|
- this.autoManager.ProcessAlarm( 21 );
|
|
|
+ this.autoManager.ProcessAlarm(21);
|
|
|
return false; //Alarm
|
|
|
}
|
|
|
|
|
|
int result = this.clamp.Unlock_Sync();
|
|
|
- if ( result != 0 )
|
|
|
+ if (result != 0)
|
|
|
{
|
|
|
- this.autoManager.ProcessAlarm( result );
|
|
|
+ this.autoManager.ProcessAlarm(result);
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
- result = this.PIOAndUnload( sub.TargetID );
|
|
|
- if ( result != 0 )
|
|
|
+ result = this.PIOAndUnload(sub.TargetID);
|
|
|
+ if (result != 0)
|
|
|
{
|
|
|
- this.autoManager.ProcessAlarm( result );
|
|
|
+ this.autoManager.ProcessAlarm(result);
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
- sql.CommandDAL.UpdateState( sub.CmdID, eCommandState.Complete );
|
|
|
- sql.SubCmdDAL.Delete( sub );
|
|
|
+ sql.CommandDAL.UpdateState(sub.CmdID, eCommandState.Complete);
|
|
|
+ sql.SubCmdDAL.Delete(sub);
|
|
|
|
|
|
return true;
|
|
|
}
|
|
|
@@ -629,7 +677,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
/// </summary>
|
|
|
/// <param name="sub"></param>
|
|
|
/// <returns></returns>
|
|
|
- public bool BatteryCharge( SubCmd sub )
|
|
|
+ public bool BatteryCharge(SubCmd sub)
|
|
|
{
|
|
|
var route = sql.RouteDal.GetRoute(sub.TargetID);
|
|
|
|
|
|
@@ -711,21 +759,21 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
#region Check Method
|
|
|
bool CheckObstacle()
|
|
|
{
|
|
|
- if ( this.iO.IsOn( "IN_OBSTRUCTION_DETECT_SAFETY" ) || this.iO.IsOn( "IN_OBSTRUCTION_DETECT_ERROR" ) )
|
|
|
+ if (this.iO.IsOn("IN_OBSTRUCTION_DETECT_SAFETY") || this.iO.IsOn("IN_OBSTRUCTION_DETECT_ERROR"))
|
|
|
{
|
|
|
this.motion.Stop();
|
|
|
this.ObstacleStateProperty = eObstacleState.Abnormal;
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
- if ( this.iO.IsOn( "IN_OBSTRUCTION_DETECT_STOP" ) )
|
|
|
+ if (this.iO.IsOn("IN_OBSTRUCTION_DETECT_STOP"))
|
|
|
{
|
|
|
this.motion.Stop();
|
|
|
this.ObstacleStateProperty = eObstacleState.Blocked;
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
- if ( this.iO.IsOn( "IN_OBSTRUCTION_DETECT_SLOW" ) )
|
|
|
+ if (this.iO.IsOn("IN_OBSTRUCTION_DETECT_SLOW"))
|
|
|
{
|
|
|
this.motion.SlowStop();
|
|
|
this.ObstacleStateProperty = eObstacleState.Decelerate;
|
|
|
@@ -742,67 +790,67 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
#region Machanical Method
|
|
|
|
|
|
#region Conveyor
|
|
|
- int OnOffConveyor( bool isOn, bool isCW = false )
|
|
|
+ int OnOffConveyor(bool isOn, bool isCW = false)
|
|
|
{
|
|
|
- if ( IsInverterError() )
|
|
|
+ if (IsInverterError())
|
|
|
return 16;
|
|
|
|
|
|
- if ( isCW )
|
|
|
- this.iO.OutputOn( "OUT_CV_CWCCW" );
|
|
|
+ if (isCW)
|
|
|
+ this.iO.OutputOn("OUT_CV_CWCCW");
|
|
|
else
|
|
|
- this.iO.OutputOff( "OUT_CV_CWCCW" );
|
|
|
+ this.iO.OutputOff("OUT_CV_CWCCW");
|
|
|
|
|
|
- if ( isOn )
|
|
|
- this.iO.OutputOn( "OUT_CV_RUN" );
|
|
|
+ if (isOn)
|
|
|
+ this.iO.OutputOn("OUT_CV_RUN");
|
|
|
else
|
|
|
- this.iO.OutputOff( "OUT_CV_RUN" );
|
|
|
+ this.iO.OutputOff("OUT_CV_RUN");
|
|
|
|
|
|
return 0;
|
|
|
}
|
|
|
|
|
|
- void SetConveyorSpeed( bool IsHight )
|
|
|
+ void SetConveyorSpeed(bool IsHight)
|
|
|
{
|
|
|
- if ( IsHight )
|
|
|
- this.iO.WriteOutputIO( "OUT_CV_DA", true );
|
|
|
+ if (IsHight)
|
|
|
+ this.iO.WriteOutputIO("OUT_CV_DA", true);
|
|
|
else
|
|
|
- this.iO.WriteOutputIO( "OUT_CV_DA", false );
|
|
|
+ this.iO.WriteOutputIO("OUT_CV_DA", false);
|
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
|
/// 입구 감지 로딩시 감속 사용
|
|
|
/// </summary>
|
|
|
/// <returns></returns>
|
|
|
- bool IsDetectedLoadStart() => this.iO.IsOn( "IN_CV_DETECT_00" );
|
|
|
+ bool IsDetectedLoadStart() => this.iO.IsOn("IN_CV_DETECT_00");
|
|
|
|
|
|
/// <summary>
|
|
|
/// 실물 감지
|
|
|
/// </summary>
|
|
|
/// <returns></returns>
|
|
|
- public bool IsDetectedCenter() => this.iO.IsOn( "IN_CV_DETECT_01" );
|
|
|
+ public bool IsDetectedCenter() => this.iO.IsOn("IN_CV_DETECT_01");
|
|
|
|
|
|
- bool IsDetectedLoadStop() => this.iO.IsOn( "IN_CV_DETECT_02" );
|
|
|
- bool IsInverterError() => this.iO.IsOn( "IN_CV_ERROR" );
|
|
|
- bool IsLifterPositinCheck() => this.iO.IsOn( "IN_LIFTER_POSITION_DETECT" );
|
|
|
- bool IsLifterDuplication() => this.iO.IsOn( "IN_LIFTER_DUPLICATION_DETECT" );
|
|
|
- bool IsPIOInterLockOn() => this.iO.IsOn( "OUT_PIO_INTERLOCK" );
|
|
|
+ bool IsDetectedLoadStop() => this.iO.IsOn("IN_CV_DETECT_02");
|
|
|
+ bool IsInverterError() => this.iO.IsOn("IN_CV_ERROR");
|
|
|
+ bool IsLifterPositinCheck() => this.iO.IsOn("IN_LIFTER_POSITION_DETECT");
|
|
|
+ bool IsLifterDuplication() => this.iO.IsOn("IN_LIFTER_DUPLICATION_DETECT");
|
|
|
+ bool IsPIOInterLockOn() => this.iO.IsOn("OUT_PIO_INTERLOCK");
|
|
|
|
|
|
int Load_Carrier()
|
|
|
{
|
|
|
- if ( IsDetectedCenter() )
|
|
|
+ if (IsDetectedCenter())
|
|
|
return 9;
|
|
|
|
|
|
- OnOffConveyor( true, true );
|
|
|
+ OnOffConveyor(true, true);
|
|
|
|
|
|
long sTime = SwUtils.CurrentTimeMillis;
|
|
|
- while ( true )
|
|
|
+ while (true)
|
|
|
{
|
|
|
- if ( SwUtils.Gt( sTime, 20 * ConstUtils.ONE_SECOND ) ) //Wait 20Sec
|
|
|
+ if (SwUtils.Gt(sTime, 20 * ConstUtils.ONE_SECOND)) //Wait 20Sec
|
|
|
{
|
|
|
- OnOffConveyor( false, true );
|
|
|
+ OnOffConveyor(false, true);
|
|
|
return 10;
|
|
|
}
|
|
|
|
|
|
- if ( IsDetectedLoadStart() )
|
|
|
+ if (IsDetectedLoadStart())
|
|
|
break;
|
|
|
}
|
|
|
|
|
|
@@ -811,34 +859,34 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
|
|
|
int UnloadCarrier()
|
|
|
{
|
|
|
- if ( !IsDetectedLoadStart() )
|
|
|
+ if (!IsDetectedLoadStart())
|
|
|
return 11;
|
|
|
|
|
|
- OnOffConveyor( true, true );
|
|
|
+ OnOffConveyor(true, true);
|
|
|
|
|
|
long sTime = SwUtils.CurrentTimeMillis;
|
|
|
- while ( true )
|
|
|
+ while (true)
|
|
|
{
|
|
|
- if ( SwUtils.Gt( sTime, 20 * ConstUtils.ONE_SECOND ) ) //Wait 20Sec
|
|
|
+ if (SwUtils.Gt(sTime, 20 * ConstUtils.ONE_SECOND)) //Wait 20Sec
|
|
|
{
|
|
|
- OnOffConveyor( false, true );
|
|
|
+ OnOffConveyor(false, true);
|
|
|
return 12;
|
|
|
}
|
|
|
|
|
|
- if ( !IsDetectedLoadStart() )
|
|
|
+ if (!IsDetectedLoadStart())
|
|
|
break;
|
|
|
}
|
|
|
|
|
|
return 0;
|
|
|
}
|
|
|
|
|
|
- public int PIOAndLoad( string targetName )
|
|
|
+ public int PIOAndLoad(string targetName)
|
|
|
{
|
|
|
#if SIMULATION
|
|
|
PIOClear();
|
|
|
loggerPIO.I($"Start Load PIO - [{targetName}]");
|
|
|
this.OnPIOStart?.Invoke(true);
|
|
|
-
|
|
|
+
|
|
|
this.iO.WriteOutputIO("OUT_PIO_RECEIVE_RUN", true);
|
|
|
loggerPIO.I("[Vehicle] - 4 Receive Run On");
|
|
|
|
|
|
@@ -957,7 +1005,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
return 0;
|
|
|
}
|
|
|
|
|
|
- public int PIOAndUnload( string targetName )
|
|
|
+ public int PIOAndUnload(string targetName)
|
|
|
{
|
|
|
#if SIMULATION
|
|
|
PIOClear();
|
|
|
@@ -1067,7 +1115,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
void PIOClear()
|
|
|
{
|
|
|
string[] pio = { "OUT_PIO_READY", "OUT_PIO_SENDING_RUN", "OUT_PIO_SEND_COMPLITE", "OUT_PIO_RECEIVABLE", "OUT_PIO_RECEIVE_RUN", "OUT_PIO_RECIVE_COMPLITE", "OUT_PIO_INTERLOCK" };
|
|
|
- pio.FwEach( x => { this.iO.OutputOff( x ); } );
|
|
|
+ pio.FwEach(x => { this.iO.OutputOff(x); });
|
|
|
}
|
|
|
|
|
|
#endregion
|
|
|
@@ -1077,13 +1125,26 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
#region Hardware Create Method
|
|
|
void CreateSteering()
|
|
|
{
|
|
|
- this.steering = new Steering( this.iO, this.sql, this.eventAggregator );
|
|
|
+ this.steering = new Steering(this.iO, this.sql, this.eventAggregator);
|
|
|
this.steering.OnSteeringError += Steering_OnSteeringError;
|
|
|
+ this.steering.PropertyChanged += Steering_PropertyChanged;
|
|
|
+ }
|
|
|
+
|
|
|
+ private void Steering_PropertyChanged(object sender, System.ComponentModel.PropertyChangedEventArgs e)
|
|
|
+ {
|
|
|
+ var property = sender.GetType().GetProperty(e.PropertyName);
|
|
|
+ var newValue = property.GetValue(sender, null);
|
|
|
+
|
|
|
+ if (e.PropertyName.Equals("SteeringState"))
|
|
|
+ {
|
|
|
+ var v = CastTo<eSteeringState>.From<object>(newValue);
|
|
|
+ this.SteeringState = v;
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
void CreateClamp()
|
|
|
{
|
|
|
- this.clamp = new Clamp( this.sql, this.eventAggregator );
|
|
|
+ this.clamp = new Clamp(this.sql, this.eventAggregator);
|
|
|
this.clamp.Init();
|
|
|
}
|
|
|
|
|
|
@@ -1097,12 +1158,12 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
/// <param name="route"></param>
|
|
|
/// <param name="currentPosition"></param>
|
|
|
/// <returns></returns>
|
|
|
- bool CorrectPosition( Route route, double currentPosition )
|
|
|
+ bool CorrectPosition(Route route, double currentPosition)
|
|
|
{
|
|
|
var rScale = route.ScaleValue;
|
|
|
var rTolerance = route.ScaleTolerance;
|
|
|
var result = currentPosition - rScale;
|
|
|
- if ( rTolerance < Math.Abs( result ) )
|
|
|
+ if (rTolerance < Math.Abs(result))
|
|
|
return false;
|
|
|
|
|
|
return true;
|
|
|
@@ -1114,19 +1175,19 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
/// </summary>
|
|
|
/// <param name="no"> 0 == Off Laser</param>
|
|
|
/// <returns></returns>
|
|
|
- bool ChgObstacleDetectPattern( int no )
|
|
|
+ bool ChgObstacleDetectPattern(int no)
|
|
|
{
|
|
|
- var bitArray = BitUtils.ChgBitArray( no );
|
|
|
+ var bitArray = BitUtils.ChgBitArray(no);
|
|
|
|
|
|
int bitIndex = 0;
|
|
|
- this.obstacleBitList.ForEach( b =>
|
|
|
- {
|
|
|
- if ( bitArray[bitIndex] )
|
|
|
- this.iO.OutputOff( b );
|
|
|
- else
|
|
|
- this.iO.OutputOn( b );
|
|
|
- bitIndex++;
|
|
|
- } );
|
|
|
+ this.obstacleBitList.ForEach(b =>
|
|
|
+ {
|
|
|
+ if (bitArray[bitIndex])
|
|
|
+ this.iO.OutputOff(b);
|
|
|
+ else
|
|
|
+ this.iO.OutputOn(b);
|
|
|
+ bitIndex++;
|
|
|
+ });
|
|
|
|
|
|
return true;
|
|
|
}
|
|
|
@@ -1135,29 +1196,29 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
|
int bitIndex = 0;
|
|
|
|
|
|
- BitArray bitArray = new BitArray( this.obstacleBitList.Count );
|
|
|
-
|
|
|
- this.obstacleBitList.ForEach( b =>
|
|
|
- {
|
|
|
- if ( this.iO.IsOn( b ) )
|
|
|
- bitArray.Set( bitIndex, false );
|
|
|
- else
|
|
|
- bitArray.Set( bitIndex, true );
|
|
|
- bitIndex++;
|
|
|
- } );
|
|
|
+ BitArray bitArray = new BitArray(this.obstacleBitList.Count);
|
|
|
|
|
|
- return BitUtils.ChgInt32( bitArray );
|
|
|
+ this.obstacleBitList.ForEach(b =>
|
|
|
+ {
|
|
|
+ if (this.iO.IsOn(b))
|
|
|
+ bitArray.Set(bitIndex, false);
|
|
|
+ else
|
|
|
+ bitArray.Set(bitIndex, true);
|
|
|
+ bitIndex++;
|
|
|
+ });
|
|
|
+
|
|
|
+ return BitUtils.ChgInt32(bitArray);
|
|
|
}
|
|
|
|
|
|
#endregion
|
|
|
|
|
|
#region Event Subscribe
|
|
|
- private void Steering_OnSteeringError( object sender, int e )
|
|
|
+ private void Steering_OnSteeringError(object sender, int e)
|
|
|
{
|
|
|
- if ( e != 0 )
|
|
|
+ if (e != 0)
|
|
|
{
|
|
|
- logger.E( $"[Steering] - Control Error {e}" );
|
|
|
- this.autoManager.ProcessAlarm( e );
|
|
|
+ logger.E($"[Steering] - Control Error {e}");
|
|
|
+ this.autoManager.ProcessAlarm(e);
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
@@ -1165,10 +1226,10 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
|
EventDir = DriveControlEventArgs.eEventDir.ToFront,
|
|
|
ControlKind = DriveControlEventArgs.eControlKind.Steering,
|
|
|
- Result = FluentResults.Results.Ok<DriveControlEventArgs.eMoveDir>( DriveControlEventArgs.eMoveDir.LEFT ),
|
|
|
+ Result = FluentResults.Results.Ok<DriveControlEventArgs.eMoveDir>(DriveControlEventArgs.eMoveDir.LEFT),
|
|
|
};
|
|
|
|
|
|
- this.eventAggregator.GetEvent<DriveControlPubSubEvent>().Publish( msg );
|
|
|
+ this.eventAggregator.GetEvent<DriveControlPubSubEvent>().Publish(msg);
|
|
|
}
|
|
|
|
|
|
}
|
ys-hwang