|
|
@@ -57,8 +57,14 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
get { return currentTag; }
|
|
|
set
|
|
|
{
|
|
|
- if ( SetField( ref this.currentTag , value ) )
|
|
|
+ if ( SetField( ref this.currentTag, value ) )
|
|
|
+ {
|
|
|
+ var info = sql.VehicleInfoDAL.GetInfo();
|
|
|
+ info.CurrentTag = value;
|
|
|
+ sql.VehicleInfoDAL.Update( info );
|
|
|
+
|
|
|
this.OnCurrentTagChanged?.Invoke( value );
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
|
|
|
@@ -71,7 +77,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
get { return currentPosition; }
|
|
|
set
|
|
|
{
|
|
|
- if ( SetField( ref this.currentPosition , value ) )
|
|
|
+ if ( SetField( ref this.currentPosition, value ) )
|
|
|
{
|
|
|
}
|
|
|
}
|
|
|
@@ -81,41 +87,41 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
public double CurrentSpeed
|
|
|
{
|
|
|
get { return currentSpeed; }
|
|
|
- set { SetField( ref this.currentSpeed , value ); }
|
|
|
+ set { SetField( ref this.currentSpeed, value ); }
|
|
|
}
|
|
|
|
|
|
private double currentTorque;
|
|
|
public double CurrentTorque
|
|
|
{
|
|
|
get { return currentTorque; }
|
|
|
- set { SetField( ref this.currentTorque , value ); }
|
|
|
+ set { SetField( ref this.currentTorque, value ); }
|
|
|
}
|
|
|
|
|
|
private bool isContain;
|
|
|
public bool IsContain
|
|
|
{
|
|
|
get { return isContain; }
|
|
|
- set { SetField( ref this.isContain , value ); }
|
|
|
+ set { SetField( ref this.isContain, value ); }
|
|
|
}
|
|
|
|
|
|
eClampState _clampState;
|
|
|
public eClampState ClampState
|
|
|
{
|
|
|
get { return this._clampState; }
|
|
|
- set { this.SetField( ref this._clampState , value ); }
|
|
|
+ set { this.SetField( ref this._clampState, value ); }
|
|
|
}
|
|
|
|
|
|
private eSteeringState steeringState;
|
|
|
public eSteeringState SteeringState
|
|
|
{
|
|
|
get { return steeringState; }
|
|
|
- set { SetField( ref this.steeringState , value ); }
|
|
|
+ set { SetField( ref this.steeringState, value ); }
|
|
|
}
|
|
|
|
|
|
private int _obstacleDrive;
|
|
|
- public int ObstacleDrive { get { return this._obstacleDrive; } set { SetField( ref this._obstacleDrive , value ); } }
|
|
|
+ public int ObstacleDrive { get { return this._obstacleDrive; } set { SetField( ref this._obstacleDrive, value ); } }
|
|
|
private int _obstacleCurve;
|
|
|
- public int ObstacleCurve { get { return this._obstacleCurve; } set { SetField( ref this._obstacleCurve , value ); } }
|
|
|
+ public int ObstacleCurve { get { return this._obstacleCurve; } set { SetField( ref this._obstacleCurve, value ); } }
|
|
|
|
|
|
private eObstacleState obstacleState = eObstacleState.Normal;
|
|
|
public eObstacleState ObstacleStateProperty
|
|
|
@@ -123,7 +129,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
get { return obstacleState; }
|
|
|
set
|
|
|
{
|
|
|
- if ( SetField( ref this.obstacleState , value ) )
|
|
|
+ if ( SetField( ref this.obstacleState, value ) )
|
|
|
{
|
|
|
if ( value == eObstacleState.Blocked )
|
|
|
this.VehicleStateProperty = eVehicleState.Blocked;
|
|
|
@@ -131,11 +137,34 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- private eVehicleState vehicleState;
|
|
|
+ private eVehicleState vehicleState = eVehicleState.None;
|
|
|
public eVehicleState VehicleStateProperty
|
|
|
{
|
|
|
get { return vehicleState; }
|
|
|
- set { SetField( ref this.vehicleState , value ); }
|
|
|
+ set
|
|
|
+ {
|
|
|
+ if ( SetField( ref this.vehicleState, value ) )
|
|
|
+ {
|
|
|
+ var info = sql.VehicleInfoDAL.GetInfo();
|
|
|
+ info.VehicleState = value;
|
|
|
+ sql.VehicleInfoDAL.Update( info );
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ private eMachineMode machineMode = eMachineMode.LocalMode;
|
|
|
+ public eMachineMode MachineMode
|
|
|
+ {
|
|
|
+ get { return machineMode; }
|
|
|
+ set
|
|
|
+ {
|
|
|
+ if (SetField(ref this.machineMode, value ) )
|
|
|
+ {
|
|
|
+ var info = sql.VehicleInfoDAL.GetInfo();
|
|
|
+ info.MachineMode = value;
|
|
|
+ sql.VehicleInfoDAL.Update( info );
|
|
|
+ }
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
//이동
|
|
|
@@ -201,7 +230,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
|
|
|
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 as EzIO;
|
|
|
this.iO.OnChangedIO += IO_OnChangedIO;
|
|
|
@@ -251,9 +280,9 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
|
var msg = new ObstacleControlEventArgs
|
|
|
{
|
|
|
- ControlKind = ObstacleControlEventArgs.eControlKind.INFO ,
|
|
|
- Drive = this.ObstacleDrive ,
|
|
|
- Curve = this.ObstacleCurve ,
|
|
|
+ ControlKind = ObstacleControlEventArgs.eControlKind.INFO,
|
|
|
+ Drive = this.ObstacleDrive,
|
|
|
+ Curve = this.ObstacleCurve,
|
|
|
ObstacleState = this.ObstacleStateProperty.ToString()
|
|
|
};
|
|
|
this.ObstacleControlEventPublish( msg );
|
|
|
@@ -266,7 +295,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
|
|
|
var reply = new ObstacleControlEventArgs
|
|
|
{
|
|
|
- ControlKind = ObstacleControlEventArgs.eControlKind.SAVE
|
|
|
+ ControlKind = ObstacleControlEventArgs.eControlKind.SAVE
|
|
|
};
|
|
|
reply.Result = Results.Ok( ObstacleControlEventArgs.eControlKind.SAVE );
|
|
|
this.ObstacleControlEventPublish( reply );
|
|
|
@@ -282,7 +311,6 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
this.eventAggregator.GetEvent<ObstacleControlPubSubEvent>().Publish( args );
|
|
|
}
|
|
|
|
|
|
-
|
|
|
private void ReceiveDriveControlEvent( DriveControlEventArgs _args )
|
|
|
{
|
|
|
if ( this.autoManager.OperationModeProperty != eOperatationMode.ManualMode )
|
|
|
@@ -351,11 +379,11 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
private void ReqConveyor( DriveControlEventArgs args )
|
|
|
{
|
|
|
if ( args.CvDir == DriveControlEventArgs.eCvDir.CW )
|
|
|
- this.OnOffConveyor( true , true );
|
|
|
+ this.OnOffConveyor( true, true );
|
|
|
else if ( args.CvDir == DriveControlEventArgs.eCvDir.CCW )
|
|
|
- this.OnOffConveyor( true , false );
|
|
|
+ this.OnOffConveyor( true, false );
|
|
|
else if ( args.CvDir == DriveControlEventArgs.eCvDir.STOP )
|
|
|
- this.OnOffConveyor( false , false );
|
|
|
+ this.OnOffConveyor( false, false );
|
|
|
}
|
|
|
|
|
|
private void DriveControlEventPublish( DriveControlEventArgs args )
|
|
|
@@ -364,7 +392,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
this.eventAggregator.GetEvent<DriveControlPubSubEvent>().Publish( args );
|
|
|
}
|
|
|
|
|
|
- public void Init( )
|
|
|
+ public void Init()
|
|
|
{
|
|
|
this.CreateClamp();
|
|
|
this.CreateSteering();
|
|
|
@@ -374,9 +402,15 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
ThreadStart();
|
|
|
|
|
|
//TimerUtils.Once(5000, () => { this.CurrentPosition = 1000; });
|
|
|
+ var v = sql.VehicleInfoDAL.GetInfo();
|
|
|
+ v.CurrentTag = "0000";
|
|
|
+ v.VehicleState = eVehicleState.None;
|
|
|
+ v.MachineMode = eMachineMode.LocalMode;
|
|
|
+
|
|
|
+ sql.VehicleInfoDAL.Update( v );
|
|
|
}
|
|
|
|
|
|
- public int InitializationVehicle( )
|
|
|
+ public int InitializationVehicle()
|
|
|
{
|
|
|
int result = 0;
|
|
|
if ( this.IsDetectedCenter() ) //자제가 있으면 Lock
|
|
|
@@ -390,7 +424,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
return result;
|
|
|
}
|
|
|
|
|
|
- public void Dispose( )
|
|
|
+ public void Dispose()
|
|
|
{
|
|
|
this.cancel.Cancel();
|
|
|
this.cancel.StopWaitAll();
|
|
|
@@ -457,9 +491,9 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
drive = "NEGATIVE";
|
|
|
}
|
|
|
|
|
|
- void ReqCurrentPos( )
|
|
|
+ void ReqCurrentPos()
|
|
|
{
|
|
|
- var task = Task.Factory.StartNew( ( ) =>
|
|
|
+ var task = Task.Factory.StartNew( () =>
|
|
|
{
|
|
|
while ( !this.taskCancel.Canceled )
|
|
|
{
|
|
|
@@ -467,9 +501,9 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
|
|
|
var msg = new DriveControlEventArgs
|
|
|
{
|
|
|
- EventDir = DriveControlEventArgs.eEventDir.ToFront ,
|
|
|
- ControlKind = DriveControlEventArgs.eControlKind.ReqCurrentPos ,
|
|
|
- CurrentPosition = new Random().Next( 0 , 1000 ) ,
|
|
|
+ EventDir = DriveControlEventArgs.eEventDir.ToFront,
|
|
|
+ ControlKind = DriveControlEventArgs.eControlKind.ReqCurrentPos,
|
|
|
+ CurrentPosition = new Random().Next( 0, 1000 ),
|
|
|
};
|
|
|
|
|
|
this.DriveControlEventPublish( msg );
|
|
|
@@ -509,7 +543,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
#endregion
|
|
|
|
|
|
#region Thread
|
|
|
- void ThreadStart( )
|
|
|
+ void ThreadStart()
|
|
|
{
|
|
|
this.cancel.AddGo( new Action( this._ThSubCmdWorker ) );
|
|
|
this.cancel.AddGo( new Action( this._ThObstacleChecker ) );
|
|
|
@@ -517,7 +551,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
|
|
|
//장애물 감지 Thread
|
|
|
//장애물 감지 패턴 변경도 여기 하자.
|
|
|
- private void _ThObstacleChecker( )
|
|
|
+ private void _ThObstacleChecker()
|
|
|
{
|
|
|
while ( !this.cancel.Canceled )
|
|
|
{
|
|
|
@@ -544,7 +578,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
/// <summary>
|
|
|
/// Scheduler 가 주는 Sub Command 를 이용하여 동작하자.
|
|
|
/// </summary>
|
|
|
- public void _ThSubCmdWorker( )
|
|
|
+ public void _ThSubCmdWorker()
|
|
|
{
|
|
|
while ( !this.cancel.Canceled )
|
|
|
{
|
|
|
@@ -610,7 +644,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
#endregion
|
|
|
|
|
|
#region Control Action Method
|
|
|
- public void EStop( )
|
|
|
+ public void EStop()
|
|
|
{
|
|
|
//Clamp EStop
|
|
|
this.clamp.ClampEStop();
|
|
|
@@ -639,32 +673,39 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
////TimerUtils.Once(3000, BuzzerOnOff, false, eBuzzerKind.StartWarn );
|
|
|
//Thread.Sleep(3000);
|
|
|
//this.BuzzerOnOff(false);
|
|
|
- this.VehicleStateProperty = eVehicleState.Move;
|
|
|
|
|
|
- this.OnMoveReady?.Invoke();
|
|
|
+ if ( this.VehicleStateProperty == eVehicleState.Idle )
|
|
|
+ {
|
|
|
+ this.OnMoveReady?.Invoke();
|
|
|
+
|
|
|
+ var moveReadyBuzzerTime = sql.ConfigDal.GetValueToInt( ConstString.BuzzerStartReadyTime );
|
|
|
+ Thread.Sleep( moveReadyBuzzerTime );
|
|
|
|
|
|
- var moveReadyBuzzerTime = sql.ConfigDal.GetValueToInt( ConstString.BuzzerStartReadyTime );
|
|
|
- Thread.Sleep( moveReadyBuzzerTime );
|
|
|
+ this.VehicleStateProperty = eVehicleState.Move;
|
|
|
+ }
|
|
|
|
|
|
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;
|
|
|
//this.BuzzerOnOff(false);
|
|
|
- this.OnMoveFinish?.Invoke();
|
|
|
+ if ( motion.IsStop )
|
|
|
+ {
|
|
|
+ this.IsMoving = false;
|
|
|
+ this.OnMoveFinish?.Invoke();
|
|
|
|
|
|
- this.VehicleStateProperty = eVehicleState.Idle;
|
|
|
+ this.VehicleStateProperty = eVehicleState.Idle;
|
|
|
+ }
|
|
|
|
|
|
return result;
|
|
|
}
|
|
|
|
|
|
- bool Wait4MoveDone( )
|
|
|
+ bool Wait4MoveDone()
|
|
|
{
|
|
|
- int waitTime = 6000; //설정 할 수있게.
|
|
|
+ int waitTime = 9000; //설정 할 수있게.
|
|
|
long st = SwUtils.CurrentTimeMillis;
|
|
|
|
|
|
//Todo: 이동시 확인 사항들.
|
|
|
@@ -672,19 +713,43 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
|
Thread.Sleep( 5 );
|
|
|
|
|
|
- if ( SwUtils.Gt( st , waitTime ) )
|
|
|
+ if ( SwUtils.Gt( st, waitTime ) )
|
|
|
{
|
|
|
//Todo: 이동시간 초과 시 동작들.
|
|
|
break;
|
|
|
}
|
|
|
|
|
|
- if ( this.ObstacleStateProperty == eObstacleState.Blocked )
|
|
|
- return false;
|
|
|
-
|
|
|
//Todo: 이동중 명령이 삭제 되면 처리 할일들.
|
|
|
- //if (!sql.SubCmdDAL.HasK(this.CurrentSubCommand.ID))
|
|
|
- //{
|
|
|
- //}
|
|
|
+ //이동중 메인 명력이 없어진다면 정지 후
|
|
|
+ if ( !sql.CommandDAL.HasK( this.CurrentSubCommand.CmdID ) )
|
|
|
+ {
|
|
|
+ logger.D( "[Wait Move Done] - 메인 명령 사라짐" );
|
|
|
+ var cmd = sql.CommandDAL.GetCmd();
|
|
|
+ if ( cmd == null )
|
|
|
+ {
|
|
|
+ logger.D( "[Wait Move Done] - Main Command not Exist Motion Stop" );
|
|
|
+ motion.Stop();
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ logger.D( "[Wait Move Done] - Main Command not Exist Motion command 없음" );
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if ( this.ObstacleStateProperty != eObstacleState.Normal )
|
|
|
+ {
|
|
|
+ if ( this.ObstacleStateProperty == eObstacleState.Blocked )
|
|
|
+ this.VehicleStateProperty = eVehicleState.Blocked;
|
|
|
+
|
|
|
+ if ( this.ObstacleStateProperty == eObstacleState.Abnormal )
|
|
|
+ {
|
|
|
+ this.VehicleStateProperty = eVehicleState.Abnormal;
|
|
|
+ this.OccurVehicleAlarm( 24 );
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
return true;
|
|
|
@@ -696,7 +761,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
|
|
|
var route = sql.RouteDal.GetRoute( sub.TargetID );
|
|
|
|
|
|
- if ( !CorrectPosition( route , this.CurrentPosition ) )
|
|
|
+ if ( !CorrectPosition( route, this.CurrentPosition ) )
|
|
|
{
|
|
|
this.OccurVehicleAlarm( 20 );
|
|
|
return false; //Alarm
|
|
|
@@ -724,7 +789,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
}
|
|
|
|
|
|
//Load, Unload 가 끝나면 메인 Command 를 완료 했다고 판단.
|
|
|
- sql.CommandDAL.UpdateState( sub.CmdID , eCommandState.Complete );
|
|
|
+ sql.CommandDAL.UpdateState( sub.CmdID, eCommandState.Complete );
|
|
|
sql.SubCmdDAL.Delete( sub );
|
|
|
|
|
|
this.VehicleStateProperty = eVehicleState.Idle;
|
|
|
@@ -738,7 +803,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
|
|
|
var route = sql.RouteDal.GetRoute( sub.TargetID );
|
|
|
|
|
|
- if ( !CorrectPosition( route , this.CurrentPosition ) )
|
|
|
+ if ( !CorrectPosition( route, this.CurrentPosition ) )
|
|
|
{
|
|
|
this.OccurVehicleAlarm( 21 );
|
|
|
return false; //Alarm
|
|
|
@@ -758,7 +823,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
- sql.CommandDAL.UpdateState( sub.CmdID , eCommandState.Complete );
|
|
|
+ sql.CommandDAL.UpdateState( sub.CmdID, eCommandState.Complete );
|
|
|
sql.SubCmdDAL.Delete( sub );
|
|
|
|
|
|
this.VehicleStateProperty = eVehicleState.Idle;
|
|
|
@@ -776,7 +841,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
|
var route = sql.RouteDal.GetRoute( sub.TargetID );
|
|
|
|
|
|
- if ( !CorrectPosition( route , this.CurrentPosition ) )
|
|
|
+ if ( !CorrectPosition( route, this.CurrentPosition ) )
|
|
|
{
|
|
|
this.OccurVehicleAlarm( 21 );
|
|
|
return false; //Alarm
|
|
|
@@ -795,10 +860,10 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
//return 0;
|
|
|
}
|
|
|
|
|
|
- this.iO.WriteOutputIO( "OUT_PIO_READY" , true );
|
|
|
+ this.iO.WriteOutputIO( "OUT_PIO_READY", true );
|
|
|
loggerPIO.I( "[Vehicle] - 1 Ready On" );
|
|
|
|
|
|
- if ( !this.iO.WaitChangeInputIO( true , pioTimeout , "IN_PIO_RECEIVE_RUN" ) )
|
|
|
+ if ( !this.iO.WaitChangeInputIO( true, pioTimeout, "IN_PIO_RECEIVE_RUN" ) )
|
|
|
{
|
|
|
PIOClear();
|
|
|
loggerPIO.E( "[Port] - 2 Receive CV Run Timeout" );
|
|
|
@@ -806,7 +871,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
//return 0;
|
|
|
}
|
|
|
|
|
|
- this.iO.WriteOutputIO( "OUT_PIO_SENDING_RUN" , true );
|
|
|
+ this.iO.WriteOutputIO( "OUT_PIO_SENDING_RUN", true );
|
|
|
loggerPIO.I( "[Vehicle] - 2 Send Run On" );
|
|
|
|
|
|
this.SetConveyorSpeed( true );
|
|
|
@@ -816,10 +881,10 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
var sTime = SwUtils.CurrentTimeMillis;
|
|
|
while ( true )
|
|
|
{
|
|
|
- if ( SwUtils.Gt( sTime , 20 * ConstUtils.ONE_SECOND ) )
|
|
|
+ if ( SwUtils.Gt( sTime, 20 * ConstUtils.ONE_SECOND ) )
|
|
|
{
|
|
|
PIOClear();
|
|
|
- this.OnOffConveyor( false , true );
|
|
|
+ this.OnOffConveyor( false, true );
|
|
|
loggerPIO.E( "[Port] Conveyor Wait Time Out" );
|
|
|
this.OnFailReport?.Invoke( eFailCode.UnlaodPIOInterlockTimeout );
|
|
|
|
|
|
@@ -840,8 +905,8 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
this.OnConveyorStop?.Invoke( false );
|
|
|
PIOClear();
|
|
|
|
|
|
- this.iO.WriteOutputIO( "OUT_PIO_SEND_COMPLITE" , true );
|
|
|
- this.iO.WriteOutputIO( "OUT_PIO_SEND_COMPLITE" , false , 1000 );
|
|
|
+ this.iO.WriteOutputIO( "OUT_PIO_SEND_COMPLITE", true );
|
|
|
+ this.iO.WriteOutputIO( "OUT_PIO_SEND_COMPLITE", false, 1000 );
|
|
|
|
|
|
this.OnUnloadComplete?.Invoke();
|
|
|
|
|
|
@@ -852,30 +917,31 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
#endregion
|
|
|
|
|
|
#region Check Method
|
|
|
- bool CheckObstacle( )
|
|
|
+ bool CheckObstacle()
|
|
|
{
|
|
|
if ( this.iO.IsOn( "IN_OBSTRUCTION_DETECT_SAFETY" ) || this.iO.IsOn( "IN_OBSTRUCTION_DETECT_ERROR" ) )
|
|
|
{
|
|
|
- this.motion.Stop();
|
|
|
this.ObstacleStateProperty = eObstacleState.Abnormal;
|
|
|
+ this.motion.SetObstacleState( this.ObstacleStateProperty );
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
if ( this.iO.IsOn( "IN_OBSTRUCTION_DETECT_STOP" ) )
|
|
|
{
|
|
|
- this.motion.Stop();
|
|
|
this.ObstacleStateProperty = eObstacleState.Blocked;
|
|
|
+ this.motion.SetObstacleState( this.ObstacleStateProperty );
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
if ( this.iO.IsOn( "IN_OBSTRUCTION_DETECT_SLOW" ) )
|
|
|
{
|
|
|
- this.motion.SlowStop();
|
|
|
this.ObstacleStateProperty = eObstacleState.Decelerate;
|
|
|
+ this.motion.SetObstacleState( this.ObstacleStateProperty );
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
this.ObstacleStateProperty = eObstacleState.Normal;
|
|
|
+ this.motion.SetObstacleState( this.ObstacleStateProperty );
|
|
|
|
|
|
return false;
|
|
|
}
|
|
|
@@ -891,7 +957,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
/// <param name="isOn"></param>
|
|
|
/// <param name="isCW"></param>
|
|
|
/// <returns></returns>
|
|
|
- int OnOffConveyor( bool isOn , bool isCW = false )
|
|
|
+ int OnOffConveyor( bool isOn, bool isCW = false )
|
|
|
{
|
|
|
if ( IsInverterError() )
|
|
|
return 16;
|
|
|
@@ -912,45 +978,45 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
void SetConveyorSpeed( bool IsHight )
|
|
|
{
|
|
|
if ( IsHight )
|
|
|
- this.iO.WriteOutputIO( "OUT_CV_DA" , true );
|
|
|
+ this.iO.WriteOutputIO( "OUT_CV_DA", true );
|
|
|
else
|
|
|
- this.iO.WriteOutputIO( "OUT_CV_DA" , false );
|
|
|
+ this.iO.WriteOutputIO( "OUT_CV_DA", false );
|
|
|
}
|
|
|
|
|
|
- bool IsCvRun( ) => this.iO.IsOn( "OUT_CV_RUN" );
|
|
|
- bool IsCvCWCCW( ) => this.iO.IsOn( "OUT_CV_CWCCW" );
|
|
|
+ bool IsCvRun() => this.iO.IsOn( "OUT_CV_RUN" );
|
|
|
+ bool IsCvCWCCW() => this.iO.IsOn( "OUT_CV_CWCCW" );
|
|
|
|
|
|
/// <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( )
|
|
|
+ int Load_Carrier()
|
|
|
{
|
|
|
if ( IsDetectedCenter() )
|
|
|
return 9;
|
|
|
|
|
|
- OnOffConveyor( true , true );
|
|
|
+ OnOffConveyor( true, true );
|
|
|
|
|
|
long sTime = SwUtils.CurrentTimeMillis;
|
|
|
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;
|
|
|
}
|
|
|
|
|
|
@@ -961,19 +1027,19 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
return 0;
|
|
|
}
|
|
|
|
|
|
- int UnloadCarrier( )
|
|
|
+ int UnloadCarrier()
|
|
|
{
|
|
|
if ( !IsDetectedLoadStart() )
|
|
|
return 11;
|
|
|
|
|
|
- OnOffConveyor( true , true );
|
|
|
+ OnOffConveyor( true, true );
|
|
|
|
|
|
long sTime = SwUtils.CurrentTimeMillis;
|
|
|
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;
|
|
|
}
|
|
|
|
|
|
@@ -988,25 +1054,25 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
|
#if SIMULATION
|
|
|
PIOClear();
|
|
|
- loggerPIO.I($"Start Load PIO - [{targetName}]");
|
|
|
- this.OnPIOStart?.Invoke(true);
|
|
|
+ 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");
|
|
|
+ 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");
|
|
|
+ Thread.Sleep( 1000 );//상대 IO 기다린다 생각.
|
|
|
+ loggerPIO.E( "[Port] - 4 Ready On" );
|
|
|
|
|
|
//Conveyor Start
|
|
|
- loggerPIO.I("[Vehicle] - Conveyor Run");
|
|
|
- this.OnConveyorStart?.Invoke(true);
|
|
|
+ loggerPIO.I( "[Vehicle] - Conveyor Run" );
|
|
|
+ this.OnConveyorStart?.Invoke( true );
|
|
|
|
|
|
- Thread.Sleep(10000);//Conveyor 구동
|
|
|
- this.OnCarrierDetected?.Invoke(true);
|
|
|
+ Thread.Sleep( 10000 );//Conveyor 구동
|
|
|
+ this.OnCarrierDetected?.Invoke( true );
|
|
|
|
|
|
PIOClear();
|
|
|
- Thread.Sleep(1000);
|
|
|
- this.OnConveyorStop?.Invoke(true);
|
|
|
+ Thread.Sleep( 1000 );
|
|
|
+ this.OnConveyorStop?.Invoke( true );
|
|
|
#else
|
|
|
var pioTimeout = sql.ConfigDal.GetValueToInt( ConstString.PIOTimeOut );
|
|
|
|
|
|
@@ -1113,20 +1179,20 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
|
#if SIMULATION
|
|
|
PIOClear();
|
|
|
- loggerPIO.I($"Start Unload PIO - [{targetName}]");
|
|
|
- this.OnPIOStart?.Invoke(false);
|
|
|
+ loggerPIO.I( $"Start Unload PIO - [{targetName}]" );
|
|
|
+ this.OnPIOStart?.Invoke( false );
|
|
|
|
|
|
- Thread.Sleep(1000);
|
|
|
+ Thread.Sleep( 1000 );
|
|
|
|
|
|
- this.iO.WriteOutputIO("OUT_PIO_READY", true);
|
|
|
- loggerPIO.I("[Vehicle] - 1 Ready On");
|
|
|
+ this.iO.WriteOutputIO( "OUT_PIO_READY", true );
|
|
|
+ loggerPIO.I( "[Vehicle] - 1 Ready On" );
|
|
|
|
|
|
- Thread.Sleep(1000);
|
|
|
- this.OnConveyorStart?.Invoke(false);
|
|
|
+ Thread.Sleep( 1000 );
|
|
|
+ this.OnConveyorStart?.Invoke( false );
|
|
|
|
|
|
- Thread.Sleep(10000);
|
|
|
- this.OnOffConveyor(false); //Stop
|
|
|
- this.OnConveyorStop?.Invoke(false);
|
|
|
+ Thread.Sleep( 10000 );
|
|
|
+ this.OnOffConveyor( false ); //Stop
|
|
|
+ this.OnConveyorStop?.Invoke( false );
|
|
|
PIOClear();
|
|
|
|
|
|
this.OnUnloadComplete?.Invoke();
|
|
|
@@ -1216,9 +1282,9 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
return 0;
|
|
|
}
|
|
|
|
|
|
- void PIOClear( )
|
|
|
+ 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" };
|
|
|
+ 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 ); } );
|
|
|
}
|
|
|
|
|
|
@@ -1227,19 +1293,19 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
#endregion
|
|
|
|
|
|
#region Hardware Create Method
|
|
|
- void CreateSteering( )
|
|
|
+ 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;
|
|
|
}
|
|
|
- void CreateClamp( )
|
|
|
+ void CreateClamp()
|
|
|
{
|
|
|
- this.clamp = new Clamp( this.sql , this.eventAggregator );
|
|
|
+ this.clamp = new Clamp( this.sql, this.eventAggregator );
|
|
|
this.clamp.Init();
|
|
|
this.clamp.PropertyChanged += Clamp_PropertyChanged;
|
|
|
}
|
|
|
- void CreateDrive( )
|
|
|
+ void CreateDrive()
|
|
|
{
|
|
|
this.motion = new GSIMotion( this.sql );
|
|
|
this.motion.PropertyChanged += Motion_PropertyChanged;
|
|
|
@@ -1274,6 +1340,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
switch ( kind )
|
|
|
{
|
|
|
case eDataKind.Volte:
|
|
|
+ this.BatteryVolt = obj.Value;
|
|
|
break;
|
|
|
case eDataKind.Current:
|
|
|
break;
|
|
|
@@ -1308,7 +1375,7 @@ 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;
|
|
|
@@ -1332,7 +1399,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
int bitIndex = 0;
|
|
|
this.obstacleBitList.ForEach( b =>
|
|
|
{
|
|
|
- if ( bitArray[ bitIndex ] )
|
|
|
+ if ( bitArray[bitIndex] )
|
|
|
this.iO.OutputOff( b );
|
|
|
else
|
|
|
this.iO.OutputOn( b );
|
|
|
@@ -1342,7 +1409,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
- int GetObstacleDetectPattern( )
|
|
|
+ int GetObstacleDetectPattern()
|
|
|
{
|
|
|
int bitIndex = 0;
|
|
|
|
|
|
@@ -1351,9 +1418,9 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
this.obstacleBitList.ForEach( b =>
|
|
|
{
|
|
|
if ( this.iO.IsOn( b ) )
|
|
|
- bitArray.Set( bitIndex , false );
|
|
|
+ bitArray.Set( bitIndex, false );
|
|
|
else
|
|
|
- bitArray.Set( bitIndex , true );
|
|
|
+ bitArray.Set( bitIndex, true );
|
|
|
bitIndex++;
|
|
|
} );
|
|
|
|
|
|
@@ -1362,11 +1429,12 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
|
|
|
void OccurVehicleAlarm( int alarmID )
|
|
|
{
|
|
|
+ this.MachineMode = eMachineMode.LocalMode;
|
|
|
this.VehicleStateProperty = eVehicleState.Abnormal;
|
|
|
this.autoManager.ProcessAlarm( alarmID );
|
|
|
}
|
|
|
|
|
|
- void SetObstaclePattern( ObstacleControlEventArgs.eControlKind state , int value )
|
|
|
+ void SetObstaclePattern( ObstacleControlEventArgs.eControlKind state, int value )
|
|
|
{
|
|
|
if ( state == ObstacleControlEventArgs.eControlKind.DRIVE )
|
|
|
{
|
|
|
@@ -1384,10 +1452,10 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
#endregion
|
|
|
|
|
|
#region Event Subscribe
|
|
|
- private void Motion_PropertyChanged( object sender , System.ComponentModel.PropertyChangedEventArgs e )
|
|
|
+ private void Motion_PropertyChanged( object sender, System.ComponentModel.PropertyChangedEventArgs e )
|
|
|
{
|
|
|
var property = sender.GetType().GetProperty( e.PropertyName );
|
|
|
- var newValue = property.GetValue( sender , null );
|
|
|
+ var newValue = property.GetValue( sender, null );
|
|
|
|
|
|
if ( e.PropertyName.Equals( "CurrentPos" ) )
|
|
|
{
|
|
|
@@ -1414,10 +1482,10 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- private void Steering_PropertyChanged( object sender , System.ComponentModel.PropertyChangedEventArgs e )
|
|
|
+ private void Steering_PropertyChanged( object sender, System.ComponentModel.PropertyChangedEventArgs e )
|
|
|
{
|
|
|
var property = sender.GetType().GetProperty( e.PropertyName );
|
|
|
- var newValue = property.GetValue( sender , null );
|
|
|
+ var newValue = property.GetValue( sender, null );
|
|
|
|
|
|
//Todo: 나중에 Test 하자
|
|
|
//var ownPropperty = this.GetType().GetProperty(e.PropertyName);
|
|
|
@@ -1428,12 +1496,12 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- private void Clamp_PropertyChanged( object sender , System.ComponentModel.PropertyChangedEventArgs e )
|
|
|
+ private void Clamp_PropertyChanged( object sender, System.ComponentModel.PropertyChangedEventArgs e )
|
|
|
{
|
|
|
//TODO:[20/03/26 ys-hwang] Clamp State
|
|
|
|
|
|
var property = sender.GetType().GetProperty( e.PropertyName );
|
|
|
- var newValue = property.GetValue( sender , null );
|
|
|
+ var newValue = property.GetValue( sender, null );
|
|
|
|
|
|
if ( e.PropertyName.Equals( "ClampState" ) )
|
|
|
{
|
|
|
@@ -1450,7 +1518,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- private void Steering_OnSteeringError( object sender , int e )
|
|
|
+ private void Steering_OnSteeringError( object sender, int e )
|
|
|
{
|
|
|
if ( e != 0 )
|
|
|
{
|
|
|
@@ -1461,9 +1529,9 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
|
var msg = new DriveControlEventArgs()
|
|
|
{
|
|
|
- EventDir = DriveControlEventArgs.eEventDir.ToFront ,
|
|
|
- ControlKind = DriveControlEventArgs.eControlKind.Steering ,
|
|
|
- Result = FluentResults.Results.Ok<DriveControlEventArgs.eMoveDir>( DriveControlEventArgs.eMoveDir.LEFT ) ,
|
|
|
+ EventDir = DriveControlEventArgs.eEventDir.ToFront,
|
|
|
+ ControlKind = DriveControlEventArgs.eControlKind.Steering,
|
|
|
+ Result = FluentResults.Results.Ok<DriveControlEventArgs.eMoveDir>( DriveControlEventArgs.eMoveDir.LEFT ),
|
|
|
};
|
|
|
|
|
|
this.eventAggregator.GetEvent<DriveControlPubSubEvent>().Publish( msg );
|