|
|
@@ -42,6 +42,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
public HostManager HostManager { get; set; }
|
|
|
public SqliteManager Sqlite { get; set; }
|
|
|
public AutoManager AutoManager { get; set; }
|
|
|
+ public ObstacleDetecter ObstacleDetecter { get; set; }
|
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
|
@@ -639,7 +640,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
ControlKind = ObstacleControlEventArgs.eControlKind.INFO,
|
|
|
Drive = this.ObstacleDrive,
|
|
|
Curve = this.ObstacleCurve,
|
|
|
- Current = this.GetObstacleDetectPattern(),
|
|
|
+ Current = this.refObjects.ObstacleDetecter.CurrentPattern,
|
|
|
ObstacleState = this.ObstacleStateProperty.ToString()
|
|
|
};
|
|
|
this.ObstacleControlEventPublish(msg);
|
|
|
@@ -1059,7 +1060,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
|
//2020.08.18.Kang. 상위 연결이 안되어 있으면 SubCmd 실행을 안하고 대기함.
|
|
|
//if (!this.refObjects.HostManager.IsConnected)
|
|
|
- //return;
|
|
|
+ //return;
|
|
|
|
|
|
var subCmd = this.refObjects.Sqlite.SubCmdDAL.GetAll().FirstOrDefault();
|
|
|
if (subCmd == null) return;
|
|
|
@@ -1266,17 +1267,10 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
Thread.Sleep(moveReadyBuzzerTime);
|
|
|
|
|
|
this.VehicleStateProperty = eVehicleState.Move;
|
|
|
-
|
|
|
- //this.ObstaclePatternChange(this.refObjects.ZmqManager.SegmentID);
|
|
|
}
|
|
|
|
|
|
this.OnMoving?.Invoke();
|
|
|
|
|
|
- //이전에 있던 작업들 종료 및 삭제
|
|
|
- //this.taskMoveCancel.Cancel();
|
|
|
- //this.taskMoveCancel.WaitAll();
|
|
|
- //this.taskMoveCancel.Add(CheckCrossPoint());
|
|
|
-
|
|
|
this.VehicleStateProperty = eVehicleState.Move;
|
|
|
|
|
|
int result = this.refObjects.Drive.MoveToPoint(sub, 100);
|
|
|
@@ -1299,7 +1293,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
|
|
|
//2020.08.13. Kang. 주행이 끝나면 장애물감지센서의 패턴을 특정 패턴으로 변경(나성권B 요청)
|
|
|
var pattern = Convert.ToInt32(this.refObjects.Sqlite.ConfigDal.GetById(ConstString.StandbyPattern).Value);
|
|
|
- this.ChgObstacleDetectPattern(pattern);
|
|
|
+ this.refObjects.ObstacleDetecter.SetObstacleDetectPattern(pattern);
|
|
|
}
|
|
|
|
|
|
return result;
|
|
|
@@ -1313,17 +1307,19 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
|
|
|
while (true)
|
|
|
{
|
|
|
- LockUtils.Wait(50); //2020.07.29. Kang 빠르게 확인 할 필요가 없으므로 5ms -> 50ms 로 변경
|
|
|
+ LockUtils.Wait(40); //2020.07.29. Kang 빠르게 확인 할 필요가 없으므로 5ms -> 50ms 로 변경
|
|
|
|
|
|
if (SwUtils.Gt(st, waitTime))
|
|
|
{
|
|
|
logger.D("Wait4MoveDone Time Over");
|
|
|
+ this.refObjects.Drive.Stop();
|
|
|
return 39;
|
|
|
}
|
|
|
|
|
|
//이동중 메인 명력이 없어진다면 정지 후
|
|
|
if (null == this.refObjects.Sqlite.CommandDAL.GetById(this.CurrentSubCommand.CmdID))
|
|
|
{
|
|
|
+ LockUtils.Wait(10);
|
|
|
logger.D("[Wait Move Done] - 메인 명령 사라짐");
|
|
|
var cmd = this.refObjects.Sqlite.CommandDAL.GetAll();
|
|
|
if (cmd == null)
|
|
|
@@ -1392,17 +1388,8 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
return false; //Alarm
|
|
|
}
|
|
|
|
|
|
- //var route = sql.RouteDal.GetRoute( sub.TargetID );
|
|
|
- //if ( !CorrectPosition( route, this.CurrentPosition ) )
|
|
|
- //{
|
|
|
- // this.OccurVehicleAlarm( 20 );
|
|
|
- // return false; //Alarm
|
|
|
- //}
|
|
|
-
|
|
|
int result = 0;
|
|
|
-
|
|
|
- //result = this.PIOAndLoad(sub.TargetID);
|
|
|
- result = this.TestLoad();
|
|
|
+ result = this.PIOAndLoad(sub.TargetID);
|
|
|
if (result != 0)
|
|
|
{
|
|
|
this.OccurVehicleAlarm(result);
|
|
|
@@ -1420,8 +1407,6 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
this.refObjects.Sqlite.CommandDAL.Update(cmd);
|
|
|
}
|
|
|
|
|
|
- //LockUtils.Wait( 1000 );
|
|
|
- //this.OnLoadComplete?.Invoke(); //일찍 주면 다음 명령을 500ms 안에 주는 현상 있음. 그러니까 천천히 주자
|
|
|
this.VehicleStateProperty = eVehicleState.Idle;
|
|
|
|
|
|
return true;
|
|
|
@@ -1438,26 +1423,10 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
return false; //Alarm
|
|
|
}
|
|
|
|
|
|
- //var route = sql.RouteDal.GetRoute( sub.TargetID );
|
|
|
-
|
|
|
- //if ( !CorrectPosition( route, this.CurrentPosition ) )
|
|
|
- //{
|
|
|
- // this.OccurVehicleAlarm( 21 );
|
|
|
- // return false; //Alarm
|
|
|
- //}
|
|
|
-
|
|
|
- //PIO 내부로 이동.
|
|
|
- //int result = this.clamp.Unlock_Sync();
|
|
|
- //if ( result != 0 )
|
|
|
- //{
|
|
|
- // this.OccurVehicleAlarm( result );
|
|
|
- // return false;
|
|
|
- //}
|
|
|
this.VehicleStateProperty = eVehicleState.Unload;
|
|
|
|
|
|
int result = 0;
|
|
|
- //result = this.PIOAndUnload(sub.TargetID);
|
|
|
- result = this.TestUnload();
|
|
|
+ result = this.PIOAndUnload(sub.TargetID);
|
|
|
if (result != 0)
|
|
|
{
|
|
|
this.refObjects.IO.OutputOn("OUT_PIO_SENSOR_ONOFF");
|
|
|
@@ -1476,8 +1445,6 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
this.refObjects.Sqlite.CommandDAL.Update(cmd);
|
|
|
}
|
|
|
|
|
|
- //LockUtils.Wait( 1000 );
|
|
|
- //this.OnUnloadComplete?.Invoke(); //일찍 주면 다음 명령을 500ms 안에 주는 현상 있음. 그러니까 천천히 주자
|
|
|
this.VehicleStateProperty = eVehicleState.Idle;
|
|
|
|
|
|
return true;
|
|
|
@@ -2392,7 +2359,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
this.OnPIOStart?.Invoke(false);
|
|
|
|
|
|
Thread.Sleep(1000);
|
|
|
-
|
|
|
+
|
|
|
int result = this.refObjects.Clamp.Unlock_Sync();
|
|
|
if (result != 0)
|
|
|
return result;
|
|
|
@@ -2520,7 +2487,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
|
|
|
var obstacle = RouteManager.Instance.Obstacles.Where(o => o.segmentID == v).Single();
|
|
|
|
|
|
- this.ChgObstacleDetectPattern(obstacle.fieldset);
|
|
|
+ this.refObjects.ObstacleDetecter.SetObstacleDetectPattern(obstacle.fieldset);
|
|
|
logger.D($"[Obstacle Pattern Chg] - {obstacle.fieldset}");
|
|
|
}
|
|
|
|
|
|
@@ -2542,56 +2509,13 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
- /// <summary>
|
|
|
- /// if no is zero, Laser Off
|
|
|
- /// bit Off, On, On, On,On Area1
|
|
|
- /// </summary>
|
|
|
- /// <param name="no"> 0 == Off Laser</param>
|
|
|
- /// <returns></returns>
|
|
|
- public bool ChgObstacleDetectPattern(int no)
|
|
|
- {
|
|
|
- var bitArray = BitUtils.ChgBitArray(no);
|
|
|
-
|
|
|
- int bitIndex = 0;
|
|
|
- this.obstacleBitList.ForEach(b =>
|
|
|
- {
|
|
|
- if (bitArray[bitIndex])
|
|
|
- this.refObjects.IO.OutputOff(b);
|
|
|
- else
|
|
|
- this.refObjects.IO.OutputOn(b);
|
|
|
- bitIndex++;
|
|
|
- });
|
|
|
-
|
|
|
- ObstaclePattern = no;
|
|
|
-
|
|
|
- return true;
|
|
|
- }
|
|
|
-
|
|
|
- public int GetObstacleDetectPattern()
|
|
|
- {
|
|
|
- int bitIndex = 0;
|
|
|
-
|
|
|
- BitArray bitArray = new BitArray(this.obstacleBitList.Count);
|
|
|
-
|
|
|
- this.obstacleBitList.ForEach(b =>
|
|
|
- {
|
|
|
- if (this.refObjects.IO.IsOn(b, false))
|
|
|
- bitArray.Set(bitIndex, false);
|
|
|
- else
|
|
|
- bitArray.Set(bitIndex, true);
|
|
|
- bitIndex++;
|
|
|
- });
|
|
|
-
|
|
|
- return BitUtils.ChgInt32(bitArray);
|
|
|
- }
|
|
|
-
|
|
|
/// <summary>
|
|
|
/// Vehicle 이동 및 동작 중 Alarm 발생 시 처리
|
|
|
/// </summary>
|
|
|
/// <param name="alarmID"></param>
|
|
|
public void OccurVehicleAlarm(int alarmID)
|
|
|
{
|
|
|
- logger.D($"[{this.GetType().Name}] - Occur Alarm Method- {alarmID}");
|
|
|
+ //logger.D($"[{this.GetType().Name}] - Occur Alarm Method- {alarmID}");
|
|
|
|
|
|
var alarm = refObjects.Alarms.FirstOrDefault(x => x.AlarmId == alarmID);
|
|
|
if (alarm != null)
|
|
|
@@ -2600,6 +2524,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
|
this.MachineMode = eMachineMode.LocalMode;
|
|
|
this.VehicleStateProperty = eVehicleState.Abnormal;
|
|
|
+ this.refObjects.Drive.Stop();
|
|
|
this.ConveyorOff();
|
|
|
}
|
|
|
}
|
|
|
@@ -2612,12 +2537,12 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
if (state == ObstacleControlEventArgs.eControlKind.DRIVE)
|
|
|
{
|
|
|
this.ObstacleDrive = value;
|
|
|
- ChgObstacleDetectPattern(this.ObstacleDrive);
|
|
|
+ this.refObjects.ObstacleDetecter.SetObstacleDetectPattern(this.ObstacleDrive);
|
|
|
}
|
|
|
else if (state == ObstacleControlEventArgs.eControlKind.CURVE)
|
|
|
{
|
|
|
this.ObstacleCurve = value;
|
|
|
- ChgObstacleDetectPattern(this.ObstacleCurve);
|
|
|
+ this.refObjects.ObstacleDetecter.SetObstacleDetectPattern(this.ObstacleDrive);
|
|
|
}
|
|
|
else
|
|
|
return;
|
|
|
@@ -2626,7 +2551,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
public eSteeringState GetESteeringState() => this.refObjects.Steering.GetSteeringState();
|
|
|
#endregion
|
|
|
|
|
|
- #region Event Subscribe
|
|
|
+ #region Event Subscribe
|
|
|
private void BMUManager_OnDisconnect(string obj)
|
|
|
{
|
|
|
this.BatteryIsConnect = false;
|
|
|
@@ -2636,7 +2561,6 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
private void BMUManager_OnConnect(string obj)
|
|
|
{
|
|
|
this.BatteryIsConnect = true;
|
|
|
- //ReqCurrentPos();
|
|
|
}
|
|
|
|
|
|
private void BMUManager_OnFirstColtd(List<ReceivedData> obj)
|
|
|
@@ -2702,7 +2626,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
//! 자동 모드 외의 모든 경우에 패턴을 30번으로 고정.
|
|
|
if (obj == eOperatationMode.AutoMode) return;
|
|
|
|
|
|
- this.ChgObstacleDetectPattern(30);
|
|
|
+ this.refObjects.ObstacleDetecter.SetObstacleDetectPattern(30);
|
|
|
logger.D($"[Obstacle Pattern Chg] - Reason Auto Mode Changed {30}");
|
|
|
}
|
|
|
|
|
|
@@ -2720,7 +2644,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
return;
|
|
|
|
|
|
var v = CastTo<int>.From<object>(newValue);
|
|
|
- this.ChgObstacleDetectPattern(v);
|
|
|
+ this.refObjects.ObstacleDetecter.SetObstacleDetectPattern(v);
|
|
|
logger.D($"[Obstacle Pattern Chg] - {v}");
|
|
|
}
|
|
|
break;
|
|
|
@@ -2811,45 +2735,6 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
}
|
|
|
break;
|
|
|
|
|
|
-
|
|
|
- //case "LinearSpeed":
|
|
|
- // {
|
|
|
- // this.LinearSpeed = CastTo<double>.From<object>(newValue);
|
|
|
- // }
|
|
|
- // break;
|
|
|
- //case "CurveSpeed":
|
|
|
- // {
|
|
|
- // this.CurveSpeed = CastTo<double>.From<object>(newValue);
|
|
|
- // }
|
|
|
- // break;
|
|
|
- //case "JogSPeed":
|
|
|
- // {
|
|
|
- // this.JogSPeed = CastTo<double>.From<object>(newValue);
|
|
|
- // }
|
|
|
- // break;
|
|
|
- //case "AccelSpeed":
|
|
|
- // {
|
|
|
- // this.AccelSpeed = CastTo<double>.From<object>(newValue);
|
|
|
- // }
|
|
|
- // break;
|
|
|
- //case "DecelSpeed":
|
|
|
- // {
|
|
|
- // this.DecelSpeed = CastTo<double>.From<object>(newValue);
|
|
|
- // }
|
|
|
- // break;
|
|
|
- //case "CreepSpeed":
|
|
|
- // {
|
|
|
- // this.CreepSpeed = CastTo<double>.From<object>(newValue);
|
|
|
- // }
|
|
|
- // break;
|
|
|
- //case "CreepDistance":
|
|
|
- // {
|
|
|
- // this.CreepDistance = CastTo<double>.From<object>(newValue);
|
|
|
- // }
|
|
|
- // break;
|
|
|
- //case "IsCanStanbyLocation":
|
|
|
- // this.IsCanStanbyLocation = CastTo<bool>.From<object>(newValue);
|
|
|
- // break;
|
|
|
default:
|
|
|
break;
|
|
|
}
|
|
|
@@ -2969,7 +2854,7 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
{
|
|
|
if (!isFirstConnected)
|
|
|
{
|
|
|
- this.ChgObstacleDetectPattern(30);
|
|
|
+ this.refObjects.ObstacleDetecter.SetObstacleDetectPattern(30);
|
|
|
isFirstConnected = true;
|
|
|
}
|
|
|
}
|