|
|
@@ -4,6 +4,7 @@ using System.Collections.Generic;
|
|
|
using System.Linq;
|
|
|
using System.Threading;
|
|
|
using GSG.NET.Concurrent;
|
|
|
+using GSG.NET.LINQ;
|
|
|
using GSG.NET.Logging;
|
|
|
using GSG.NET.Quartz;
|
|
|
using GSG.NET.Utils;
|
|
|
@@ -24,15 +25,39 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
/// </summary>
|
|
|
public class Vehicle : ControlObjectBase, IDisposable
|
|
|
{
|
|
|
+ /// <summary>
|
|
|
+ /// OCS Report Code
|
|
|
+ /// 목적지에 도착해서 Load, Unload 시 발생하는 Alarm
|
|
|
+ /// </summary>
|
|
|
+ public enum eFailCode
|
|
|
+ {
|
|
|
+ Load_PortHasNotCarrier = 1,
|
|
|
+ Load_VehicleHasCarrier,
|
|
|
+ Unload_PortHasCarrier,
|
|
|
+ Unload_VehicleHasNotCarrier,
|
|
|
+ LoadPIOInterlockTimeout,
|
|
|
+ UnlaodPIOInterlockTimeout,
|
|
|
+ }
|
|
|
+
|
|
|
static Logger logger = Logger.GetLogger();
|
|
|
+ static Logger loggerPIO = Logger.GetLogger( "PIO" );
|
|
|
|
|
|
#region Properties
|
|
|
private double currentPosition;
|
|
|
|
|
|
+ /// <summary>
|
|
|
+ /// Tag 위치
|
|
|
+ /// </summary>
|
|
|
public double CurrentPosition
|
|
|
{
|
|
|
get { return currentPosition; }
|
|
|
- set { currentPosition = value; }
|
|
|
+ set
|
|
|
+ {
|
|
|
+ if ( this.currentPosition == value ) return;
|
|
|
+
|
|
|
+ this.currentPosition = value;
|
|
|
+ this.OnCurrentPotisionChanged?.Invoke( (int)value );
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
//이동
|
|
|
@@ -48,7 +73,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
public bool IsMoving { get; set; }
|
|
|
public double BatteryVolt { get; set; }
|
|
|
public bool IsError { get; set; }
|
|
|
- public bool IsContain { get { return this.conveyor.IsDetectedCenter(); } }
|
|
|
+ public bool IsContain { get { return this.IsDetectedCenter(); } }
|
|
|
|
|
|
public SubCmd CurrentSubCommand { get; private set; }
|
|
|
#endregion
|
|
|
@@ -76,13 +101,14 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
public event Action OnManualLoad;
|
|
|
public event Action OnManualUnload;
|
|
|
public event Action OnManualCharging;
|
|
|
+
|
|
|
+ public event Action<eFailCode> OnFailReport;
|
|
|
#endregion
|
|
|
|
|
|
IIO iO = null;
|
|
|
GSIMotion motion = null;
|
|
|
SqliteManager sql = null;
|
|
|
Clamp clamp = null;
|
|
|
- Conveyor conveyor = null;
|
|
|
Steering steering = null;
|
|
|
AutoManager autoManager = null;
|
|
|
|
|
|
@@ -98,7 +124,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
public eObstacleState ObstacleStateProperty
|
|
|
{
|
|
|
get { return obstacleState; }
|
|
|
- set { SetField(ref this.obstacleState, value); }
|
|
|
+ set { SetField( ref this.obstacleState, value ); }
|
|
|
}
|
|
|
|
|
|
private eVehicleState vehicleState;
|
|
|
@@ -106,50 +132,50 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
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;
|
|
|
|
|
|
- this.eventAggregator.GetEvent<DirveControlPubSubEvent>().Unsubscribe(ReceiveDriveControlEvent);
|
|
|
- this.eventAggregator.GetEvent<DirveControlPubSubEvent>().Subscribe(ReceiveDriveControlEvent);
|
|
|
+ this.eventAggregator.GetEvent<DirveControlPubSubEvent>().Unsubscribe( ReceiveDriveControlEvent );
|
|
|
+ this.eventAggregator.GetEvent<DirveControlPubSubEvent>().Subscribe( ReceiveDriveControlEvent );
|
|
|
}
|
|
|
|
|
|
- private void ReceiveDriveControlEvent(DriveControlEventArgs obj)
|
|
|
+ private void ReceiveDriveControlEvent( DriveControlEventArgs obj )
|
|
|
{
|
|
|
- if (this.autoManager.OperationModeProperty != eOperatationMode.ManualMode)
|
|
|
+ if ( this.autoManager.OperationModeProperty != eOperatationMode.ManualMode )
|
|
|
return;
|
|
|
|
|
|
var msg = obj;
|
|
|
- if (msg.EventDir == DriveControlEventArgs.eEventDir.ToBack)
|
|
|
+ if ( msg.EventDir == DriveControlEventArgs.eEventDir.ToBack )
|
|
|
{
|
|
|
- switch (msg.ControlKind)
|
|
|
+ switch ( msg.ControlKind )
|
|
|
{
|
|
|
case DriveControlEventArgs.eControlKind.MOVE:
|
|
|
break;
|
|
|
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;
|
|
|
@@ -157,11 +183,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;
|
|
|
default:
|
|
|
@@ -170,21 +196,34 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- private void DriveControlEventPublish(DriveControlEventArgs args)
|
|
|
+ private void DriveControlEventPublish( DriveControlEventArgs args )
|
|
|
{
|
|
|
args.EventDir = DriveControlEventArgs.eEventDir.ToFront;
|
|
|
- this.eventAggregator.GetEvent<DirveControlPubSubEvent>().Publish(args);
|
|
|
+ this.eventAggregator.GetEvent<DirveControlPubSubEvent>().Publish( args );
|
|
|
}
|
|
|
|
|
|
public void Init()
|
|
|
{
|
|
|
this.CreateClamp();
|
|
|
- this.CreateConveyor();
|
|
|
this.CreateSteering();
|
|
|
|
|
|
ThreadStart();
|
|
|
}
|
|
|
|
|
|
+ public int InitializationVehicle()
|
|
|
+ {
|
|
|
+ int result = 0;
|
|
|
+ if ( this.IsDetectedCenter() ) //자제가 있으면 Lock
|
|
|
+ result = this.clamp.Lock_Sync();
|
|
|
+ else
|
|
|
+ result = this.clamp.Unlock_Sync();
|
|
|
+
|
|
|
+ if ( this.motion.IsErrorOn )
|
|
|
+ return 22;
|
|
|
+
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+
|
|
|
public void Dispose()
|
|
|
{
|
|
|
this.cancel.Cancel();
|
|
|
@@ -194,34 +233,34 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
#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>
|
|
|
@@ -229,103 +268,109 @@ 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
|
|
|
|
|
|
#region Control Action Method
|
|
|
|
|
|
- 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 )
|
|
|
+ {
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- bool MoveTo(string pointID)
|
|
|
+ bool MoveTo( string pointID )
|
|
|
{
|
|
|
- ////ToDo: 부저를 울리는 시간을 설정에 넣자.
|
|
|
//this.BuzzerOnOff(true, eBuzzerKind.StartWarn);
|
|
|
////TimerUtils.Once(3000, BuzzerOnOff, false, eBuzzerKind.StartWarn );
|
|
|
//Thread.Sleep(3000);
|
|
|
//this.BuzzerOnOff(false);
|
|
|
+
|
|
|
this.OnMoveReady?.Invoke();
|
|
|
- Thread.Sleep(3000);
|
|
|
+
|
|
|
+ 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 );
|
|
|
|
|
|
- Wait4MoveDone();
|
|
|
+ bool result = Wait4MoveDone();
|
|
|
this.IsMoving = false;
|
|
|
//this.BuzzerOnOff(false);
|
|
|
this.OnMoveFinish?.Invoke();
|
|
|
|
|
|
- return true;
|
|
|
+ return result;
|
|
|
}
|
|
|
|
|
|
bool Wait4MoveDone()
|
|
|
@@ -334,84 +379,90 @@ 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;
|
|
|
}
|
|
|
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.conveyor.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;
|
|
|
}
|
|
|
|
|
|
+ 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.conveyor.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 );
|
|
|
+
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
- public bool BatteryCharge(SubCmd sub)
|
|
|
+ public bool BatteryCharge( SubCmd sub )
|
|
|
{
|
|
|
|
|
|
return true;
|
|
|
@@ -422,21 +473,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;
|
|
|
@@ -451,25 +502,353 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
#endregion
|
|
|
|
|
|
#region Machanical Method
|
|
|
+
|
|
|
+ #region Conveyor
|
|
|
+ int OnOffConveyor( bool isOn, bool isCW = false )
|
|
|
+ {
|
|
|
+ if ( IsInverterError() )
|
|
|
+ return 16;
|
|
|
+
|
|
|
+ if ( isCW )
|
|
|
+ this.iO.OutputOn( "OUT_CV_CWCCW" );
|
|
|
+ else
|
|
|
+ this.iO.OutputOff( "OUT_CV_CWCCW" );
|
|
|
+
|
|
|
+ if ( isOn )
|
|
|
+ this.iO.OutputOn( "OUT_CV_RUN" );
|
|
|
+ else
|
|
|
+ this.iO.OutputOff( "OUT_CV_RUN" );
|
|
|
+
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ void SetConveyorSpeed( bool IsHight )
|
|
|
+ {
|
|
|
+ if ( IsHight )
|
|
|
+ this.iO.WriteOutputIO( "OUT_CV_DA", true );
|
|
|
+ else
|
|
|
+ this.iO.WriteOutputIO( "OUT_CV_DA", false );
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 입구 감지 로딩시 감속 사용
|
|
|
+ /// </summary>
|
|
|
+ /// <returns></returns>
|
|
|
+ bool IsDetectedLoadStart() => this.iO.IsOn( "IN_CV_DETECT_00" );
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 실물 감지
|
|
|
+ /// </summary>
|
|
|
+ /// <returns></returns>
|
|
|
+ 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" );
|
|
|
+
|
|
|
+ int Load_Carrier()
|
|
|
+ {
|
|
|
+ if ( IsDetectedCenter() )
|
|
|
+ return 9;
|
|
|
+
|
|
|
+ OnOffConveyor( true, true );
|
|
|
+
|
|
|
+ long sTime = SwUtils.CurrentTimeMillis;
|
|
|
+ while ( true )
|
|
|
+ {
|
|
|
+ if ( SwUtils.Gt( sTime, 20 * ConstUtils.ONE_SECOND ) ) //Wait 20Sec
|
|
|
+ {
|
|
|
+ OnOffConveyor( false, true );
|
|
|
+ return 10;
|
|
|
+ }
|
|
|
+
|
|
|
+ if ( IsDetectedLoadStart() )
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ int UnloadCarrier()
|
|
|
+ {
|
|
|
+ if ( !IsDetectedLoadStart() )
|
|
|
+ return 11;
|
|
|
+
|
|
|
+ OnOffConveyor( true, true );
|
|
|
+
|
|
|
+ long sTime = SwUtils.CurrentTimeMillis;
|
|
|
+ while ( true )
|
|
|
+ {
|
|
|
+ if ( SwUtils.Gt( sTime, 20 * ConstUtils.ONE_SECOND ) ) //Wait 20Sec
|
|
|
+ {
|
|
|
+ OnOffConveyor( false, true );
|
|
|
+ return 12;
|
|
|
+ }
|
|
|
+
|
|
|
+ if ( !IsDetectedLoadStart() )
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ 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");
|
|
|
+
|
|
|
+ Thread.Sleep(1000);//상대 IO 기다린다 생각.
|
|
|
+ loggerPIO.E("[Port] - 4 Ready On");
|
|
|
+
|
|
|
+ //Conveyor Start
|
|
|
+ loggerPIO.I("[Vehicle] - Conveyor Run");
|
|
|
+ this.OnConveyorStart?.Invoke(true);
|
|
|
+
|
|
|
+ Thread.Sleep(10000);//Conveyor 구동
|
|
|
+ this.OnCarrierDetected?.Invoke(true);
|
|
|
+
|
|
|
+ PIOClear();
|
|
|
+ Thread.Sleep(1000);
|
|
|
+ this.OnConveyorStop?.Invoke(true);
|
|
|
+#else
|
|
|
+ var pioTimeout = sql.ConfigDal.GetValueToInt( ConstString.PIOTimeOut );
|
|
|
+
|
|
|
+ if ( this.IsInverterError() )
|
|
|
+ return 16;
|
|
|
+
|
|
|
+ if ( this.IsLifterPositinCheck() )
|
|
|
+ return 14;
|
|
|
+
|
|
|
+ if ( !this.IsLifterDuplication() )
|
|
|
+ {
|
|
|
+ this.OnFailReport?.Invoke( eFailCode.Load_PortHasNotCarrier );
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if ( this.IsDetectedCenter() )
|
|
|
+ {
|
|
|
+ this.OnFailReport?.Invoke( eFailCode.Load_VehicleHasCarrier );
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ 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" );
|
|
|
+
|
|
|
+ if ( !this.iO.WaitChangeInputIO( true, pioTimeout, "IN_PIO_SENDABLE" ) )
|
|
|
+ {
|
|
|
+ PIOClear();
|
|
|
+ loggerPIO.E( "[Port] - 4 Ready Time Out" );
|
|
|
+ this.OnFailReport?.Invoke( eFailCode.LoadPIOInterlockTimeout );
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+ loggerPIO.E( "[Port] - 4 Ready On" );
|
|
|
+
|
|
|
+ this.SetConveyorSpeed( true );
|
|
|
+ this.OnOffConveyor( true, true );
|
|
|
+ this.iO.WriteOutputIO( "OUT_PIO_RECEIVE_RUN", true );
|
|
|
+ loggerPIO.I( "[Vehicle] - Conveyor Run" );
|
|
|
+ this.OnConveyorStart?.Invoke( true );
|
|
|
+
|
|
|
+ if ( !this.iO.WaitChangeInputIO( true, pioTimeout, "IN_PIO_SEND_RUN" ) )
|
|
|
+ {
|
|
|
+ this.OnOffConveyor( false, true );
|
|
|
+ PIOClear();
|
|
|
+ loggerPIO.E( "[Port] - 5 Sending Run Time Out" );
|
|
|
+ this.OnFailReport?.Invoke( eFailCode.LoadPIOInterlockTimeout );
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ bool isStartDetected = false;
|
|
|
+ var sTime = SwUtils.CurrentTimeMillis;
|
|
|
+ while ( true )
|
|
|
+ {
|
|
|
+ if ( SwUtils.Gt( sTime, 20 * ConstUtils.ONE_SECOND ) )
|
|
|
+ {
|
|
|
+ PIOClear();
|
|
|
+ this.OnOffConveyor( false, true );
|
|
|
+ loggerPIO.E( "[Vehicle] Conveyor Wait Time Out" );
|
|
|
+ this.OnFailReport?.Invoke( eFailCode.LoadPIOInterlockTimeout );
|
|
|
+
|
|
|
+ if ( this.IsDetectedLoadStart() ) // 감지가 시작 되었으면 이동중 Error 로 판단 설비를 정지 상태로
|
|
|
+ return 10; //Conveyor Moving Timeout
|
|
|
+ else
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if ( this.IsDetectedLoadStart() && !isStartDetected )
|
|
|
+ isStartDetected = true;
|
|
|
+
|
|
|
+ if ( !this.IsDetectedLoadStart() && isStartDetected )
|
|
|
+ this.SetConveyorSpeed( false );
|
|
|
+
|
|
|
+ if ( this.IsDetectedLoadStop() ) break;
|
|
|
+
|
|
|
+ if ( this.IsPIOInterLockOn() )
|
|
|
+ {
|
|
|
+ PIOClear();
|
|
|
+ this.OnOffConveyor( false ); //Stop
|
|
|
+ loggerPIO.E( "[Port] PIO InterLock On " );
|
|
|
+ return 19; //
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if ( this.IsDetectedCenter() )
|
|
|
+ this.OnCarrierDetected?.Invoke( true );
|
|
|
+
|
|
|
+ this.OnOffConveyor( false ); //Stop
|
|
|
+ PIOClear();
|
|
|
+ this.OnConveyorStop?.Invoke( true );
|
|
|
+
|
|
|
+ this.iO.WriteOutputIO( "OUT_PIO_RECIVE_COMPLITE", true );
|
|
|
+ this.iO.WriteOutputIO( "OUT_PIO_RECIVE_COMPLITE", false, 1000 );
|
|
|
+
|
|
|
+ this.OnLoadComplete?.Invoke();
|
|
|
+#endif
|
|
|
+
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ public int PIOAndUnload( string targetName )
|
|
|
+ {
|
|
|
+#if SIMULATION
|
|
|
+ PIOClear();
|
|
|
+ loggerPIO.I($"Start Unload PIO - [{targetName}]");
|
|
|
+ this.OnPIOStart?.Invoke(false);
|
|
|
+
|
|
|
+ Thread.Sleep(1000);
|
|
|
+
|
|
|
+ this.iO.WriteOutputIO("OUT_PIO_READY", true);
|
|
|
+ loggerPIO.I("[Vehicle] - 1 Ready On");
|
|
|
+
|
|
|
+ Thread.Sleep(1000);
|
|
|
+ this.OnConveyorStart?.Invoke(false);
|
|
|
+
|
|
|
+ Thread.Sleep(10000);
|
|
|
+ this.OnOffConveyor(false); //Stop
|
|
|
+ this.OnConveyorStop?.Invoke(false);
|
|
|
+ PIOClear();
|
|
|
+
|
|
|
+ this.OnUnloadComplete?.Invoke();
|
|
|
+#else
|
|
|
+ var pioTimeout = sql.ConfigDal.GetValueToInt( ConstString.PIOTimeOut );
|
|
|
+
|
|
|
+ if ( this.IsInverterError() )
|
|
|
+ return 16;
|
|
|
+
|
|
|
+ if ( this.IsLifterDuplication() )
|
|
|
+ {
|
|
|
+ this.OnFailReport?.Invoke( eFailCode.Unload_PortHasCarrier );
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if ( !this.IsDetectedCenter() )
|
|
|
+ {
|
|
|
+ this.OnFailReport?.Invoke( eFailCode.Unload_VehicleHasNotCarrier );
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if ( this.IsLifterPositinCheck() )
|
|
|
+ return 13;
|
|
|
+
|
|
|
+ PIOClear();
|
|
|
+ loggerPIO.I( $"Start Unload PIO - [{targetName}]" );
|
|
|
+ this.OnPIOStart?.Invoke( false );
|
|
|
+
|
|
|
+ if ( !this.iO.IsOn( "IN_PIO_READY" ) )
|
|
|
+ {
|
|
|
+ loggerPIO.E( "[Port] - 1 Ready not On" );
|
|
|
+ this.OnFailReport?.Invoke( eFailCode.UnlaodPIOInterlockTimeout );
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ this.iO.WriteOutputIO( "OUT_PIO_READY", true );
|
|
|
+ loggerPIO.I( "[Vehicle] - 1 Ready On" );
|
|
|
+
|
|
|
+ if ( !this.iO.WaitChangeInputIO( true, pioTimeout, "IN_PIO_RECEIVE_RUN" ) )
|
|
|
+ {
|
|
|
+ PIOClear();
|
|
|
+ loggerPIO.E( "[Port] - 2 Receive CV Run Timeout" );
|
|
|
+ this.OnFailReport?.Invoke( eFailCode.UnlaodPIOInterlockTimeout );
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ this.iO.WriteOutputIO( "OUT_PIO_SENDING_RUN", true );
|
|
|
+ loggerPIO.I( "[Vehicle] - 2 Send Run On" );
|
|
|
+
|
|
|
+ this.SetConveyorSpeed( true );
|
|
|
+ this.OnOffConveyor( true );
|
|
|
+ this.OnConveyorStart?.Invoke( false );
|
|
|
+
|
|
|
+ var sTime = SwUtils.CurrentTimeMillis;
|
|
|
+ while ( true )
|
|
|
+ {
|
|
|
+ if ( SwUtils.Gt( sTime, 20 * ConstUtils.ONE_SECOND ) )
|
|
|
+ {
|
|
|
+ PIOClear();
|
|
|
+ this.OnOffConveyor( false, true );
|
|
|
+ loggerPIO.E( "[Port] Conveyor Wait Time Out" );
|
|
|
+ this.OnFailReport?.Invoke( eFailCode.UnlaodPIOInterlockTimeout );
|
|
|
+
|
|
|
+ if ( IsDetectedLoadStart() || IsDetectedCenter() ) //중간에 걸려 있다고 생각해서 알람 처리.
|
|
|
+ return 12; //Conveyor Moving Timeout
|
|
|
+ else
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if ( this.iO.IsOn( "IN_PIO_RECEIVE_COMPLITE" ) )
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ if ( !IsDetectedCenter() )
|
|
|
+ this.OnCarrierDetected?.Invoke( false );
|
|
|
+
|
|
|
+ this.OnOffConveyor( false ); //Stop
|
|
|
+ this.OnConveyorStop?.Invoke( false );
|
|
|
+ PIOClear();
|
|
|
+
|
|
|
+ this.iO.WriteOutputIO( "OUT_PIO_SEND_COMPLITE", true );
|
|
|
+ this.iO.WriteOutputIO( "OUT_PIO_SEND_COMPLITE", false, 1000 );
|
|
|
+
|
|
|
+ this.OnUnloadComplete?.Invoke();
|
|
|
+#endif
|
|
|
+
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ 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 ); } );
|
|
|
+ }
|
|
|
+
|
|
|
+ #endregion
|
|
|
+
|
|
|
#endregion
|
|
|
|
|
|
#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;
|
|
|
}
|
|
|
|
|
|
void CreateClamp()
|
|
|
{
|
|
|
- this.clamp = new Clamp(this.sql, this.eventAggregator);
|
|
|
+ this.clamp = new Clamp( this.sql, this.eventAggregator );
|
|
|
this.clamp.Init();
|
|
|
}
|
|
|
|
|
|
- void CreateConveyor()
|
|
|
- {
|
|
|
- this.conveyor = new Conveyor(this.iO);
|
|
|
- }
|
|
|
#endregion
|
|
|
|
|
|
#region Help Method
|
|
|
@@ -480,12 +859,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;
|
|
|
@@ -497,19 +876,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;
|
|
|
}
|
|
|
@@ -518,29 +897,29 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
|
int bitIndex = 0;
|
|
|
|
|
|
- BitArray bitArray = new BitArray(this.obstacleBitList.Count);
|
|
|
+ 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++;
|
|
|
- });
|
|
|
+ this.obstacleBitList.ForEach( b =>
|
|
|
+ {
|
|
|
+ if ( this.iO.IsOn( b ) )
|
|
|
+ bitArray.Set( bitIndex, false );
|
|
|
+ else
|
|
|
+ bitArray.Set( bitIndex, true );
|
|
|
+ bitIndex++;
|
|
|
+ } );
|
|
|
|
|
|
- return BitUtils.ChgInt32(bitArray);
|
|
|
+ return BitUtils.ChgInt32( bitArray );
|
|
|
}
|
|
|
|
|
|
#endregion
|
|
|
|
|
|
- #region Event
|
|
|
- private void Steering_OnSteeringError(object sender, int e)
|
|
|
+ #region Event Subscribe
|
|
|
+ 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
|
|
|
{
|
|
|
@@ -548,10 +927,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<DirveControlPubSubEvent>().Publish(msg);
|
|
|
+ this.eventAggregator.GetEvent<DirveControlPubSubEvent>().Publish( msg );
|
|
|
}
|
|
|
|
|
|
}
|
ys-hwang