|
@@ -18,14 +18,33 @@ namespace VirgoRT.Devices
|
|
|
{
|
|
|
private readonly DIAccessor _diOrigin;//是否到达原点
|
|
|
private readonly DIAccessor _diOrigin2;
|
|
|
+ private readonly DIAccessor _diOrigin3;
|
|
|
private readonly DIAccessor _diP1;//是否到达位置1
|
|
|
private readonly DIAccessor _diP2;//是否到达位置2
|
|
|
private readonly DIAccessor _diP3;//是否到达位置3
|
|
|
|
|
|
private readonly DIAccessor _diCOMMAlarm;//去位置1,2,3超时
|
|
|
private readonly DIAccessor _diBatteryLowAlarm;//去原点超时
|
|
|
- // private readonly DIAccessor _diServoAlarm; // servo alarm
|
|
|
- // private readonly DIAccessor _diCCWLimitSensorAlarm; //
|
|
|
+
|
|
|
+ private readonly DIAccessor _di1_L_Servo_Driver_Alarm;
|
|
|
+ private readonly DIAccessor _di1_L_Servo_Driver_Warning;
|
|
|
+ private readonly DIAccessor _di1_Search_ORG_Fail_Alarm;
|
|
|
+ private readonly DIAccessor _di1_ABS_Fail_Alarm;
|
|
|
+ private readonly DIAccessor _di1_Up_Limit_Alarm;
|
|
|
+ private readonly DIAccessor _di1_Down_Limit_Alarm;
|
|
|
+ private readonly DIAccessor _di1_Over_Software_Alarm;
|
|
|
+
|
|
|
+ private readonly DIAccessor _di2_L_Servo_Driver_Alarm;
|
|
|
+ private readonly DIAccessor _di2_L_Servo_Driver_Warning;
|
|
|
+ private readonly DIAccessor _di2_Search_ORG_Fail_Alarm;
|
|
|
+ private readonly DIAccessor _di2_ABS_Fail_Alarm;
|
|
|
+ private readonly DIAccessor _di2_Up_Limit_Alarm;
|
|
|
+ private readonly DIAccessor _di2_Down_Limit_Alarm;
|
|
|
+ private readonly DIAccessor _di2_Over_Software_Alarm;
|
|
|
+
|
|
|
+
|
|
|
+ // private readonly DIAccessor _diServoAlarm; // servo alarm
|
|
|
+ // private readonly DIAccessor _diCCWLimitSensorAlarm; //
|
|
|
//private readonly DIAccessor _diOverSoftwareLimitAlarm; //
|
|
|
|
|
|
private readonly DOAccessor _doReset;
|
|
@@ -56,13 +75,15 @@ namespace VirgoRT.Devices
|
|
|
private readonly AOAccessor _aoSoftDownLimit;
|
|
|
private readonly AOAccessor _aoCorrectionValue;
|
|
|
|
|
|
+ private Stopwatch swOrigin = new Stopwatch();
|
|
|
private Stopwatch sw = new Stopwatch();
|
|
|
private Stopwatch _manualStopTimer = new Stopwatch();
|
|
|
private readonly int _stopButtonAutoResetTime = 1000;
|
|
|
|
|
|
private Position _currentTarget = Position.Invalid;
|
|
|
+ private Position _preTarget = Position.Invalid;
|
|
|
|
|
|
- long _timeout = 10000;
|
|
|
+ long _timeout = 60000;
|
|
|
private bool _bAlarmReported = false;
|
|
|
|
|
|
|
|
@@ -87,13 +108,13 @@ namespace VirgoRT.Devices
|
|
|
{
|
|
|
get
|
|
|
{
|
|
|
- if (_diP1.Value && _diP2.Value == false && _diP3.Value == false)
|
|
|
+ if ((_preTarget == Position.position1 && _diP1.Value) && _diP2.Value == false && _diP3.Value == false)
|
|
|
return MovementPosition.Up;
|
|
|
- else if (_diP1.Value == false && _diP2.Value && _diP3.Value == false)
|
|
|
+ else if (_diP1.Value == false && (_diP2.Value && _preTarget == Position.position2) && _diP3.Value == false)
|
|
|
return MovementPosition.Middle;
|
|
|
- else if (_diP1.Value == false && _diP2.Value == false && _diP3.Value)
|
|
|
+ else if (_diP1.Value == false && _diP2.Value == false && (_diP3.Value && _preTarget == Position.position3))
|
|
|
return MovementPosition.Down;
|
|
|
- else if (_diOrigin.Value && _diOrigin2.Value)
|
|
|
+ else if (_diOrigin.Value && _diOrigin2.Value && _diOrigin3.Value && _preTarget == Position.origin)
|
|
|
return MovementPosition.Origin;
|
|
|
|
|
|
return MovementPosition.Unknown;
|
|
@@ -127,6 +148,7 @@ namespace VirgoRT.Devices
|
|
|
|
|
|
_diOrigin = ParseDiNode("diOrigin", node, ioModule);
|
|
|
_diOrigin2 = ParseDiNode("diOrigin2", node, ioModule);
|
|
|
+ _diOrigin3 = ParseDiNode("diOrigin3", node, ioModule);
|
|
|
_diP1 = ParseDiNode("diP1", node, ioModule);
|
|
|
_diP2 = ParseDiNode("diP2", node, ioModule);
|
|
|
_diP3 = ParseDiNode("diP3", node, ioModule);
|
|
@@ -134,6 +156,22 @@ namespace VirgoRT.Devices
|
|
|
_diCOMMAlarm = ParseDiNode("diCOMMAlarm", node, ioModule);
|
|
|
_diBatteryLowAlarm = ParseDiNode("diBatteryLowAlarm", node, ioModule);
|
|
|
|
|
|
+ _di1_L_Servo_Driver_Alarm = ParseDiNode("di1_L_Servo_Driver_Alarm", node, ioModule);
|
|
|
+ _di1_L_Servo_Driver_Warning = ParseDiNode("di1_L_Servo_Driver_Warning", node, ioModule);
|
|
|
+ _di1_Search_ORG_Fail_Alarm = ParseDiNode("di1_Search_ORG_Fail_Alarm", node, ioModule);
|
|
|
+ _di1_ABS_Fail_Alarm = ParseDiNode("di1_ABS_Fail_Alarm", node, ioModule);
|
|
|
+ _di1_Up_Limit_Alarm = ParseDiNode("di1_Up_Limit_Alarm", node, ioModule);
|
|
|
+ _di1_Down_Limit_Alarm = ParseDiNode("di1_Down_Limit_Alarm", node, ioModule);
|
|
|
+ _di1_Over_Software_Alarm = ParseDiNode("di1_Over_Software_Alarm", node, ioModule);
|
|
|
+
|
|
|
+ _di2_L_Servo_Driver_Alarm = ParseDiNode("di2_L_Servo_Driver_Alarm", node, ioModule);
|
|
|
+ _di2_L_Servo_Driver_Warning = ParseDiNode("di2_L_Servo_Driver_Warning", node, ioModule);
|
|
|
+ _di2_Search_ORG_Fail_Alarm = ParseDiNode("di2_Search_ORG_Fail_Alarm", node, ioModule);
|
|
|
+ _di2_ABS_Fail_Alarm = ParseDiNode("di2_ABS_Fail_Alarm", node, ioModule);
|
|
|
+ _di2_Up_Limit_Alarm = ParseDiNode("di2_Up_Limit_Alarm", node, ioModule);
|
|
|
+ _di2_Down_Limit_Alarm = ParseDiNode("di2_Down_Limit_Alarm", node, ioModule);
|
|
|
+ _di2_Over_Software_Alarm = ParseDiNode("di2_Over_Software_Alarm", node, ioModule);
|
|
|
+
|
|
|
_doReset = ParseDoNode("doReset", node, ioModule);
|
|
|
|
|
|
_doOrigin = ParseDoNode("doOrigin", node, ioModule);
|
|
@@ -190,7 +228,9 @@ namespace VirgoRT.Devices
|
|
|
return false;
|
|
|
|
|
|
_currentTarget = position;
|
|
|
+ _preTarget = position;
|
|
|
sw.Restart();
|
|
|
+ swOrigin.Restart();
|
|
|
switch (position)
|
|
|
{
|
|
|
case Position.position1:
|
|
@@ -212,12 +252,12 @@ namespace VirgoRT.Devices
|
|
|
break;
|
|
|
case Position.origin:
|
|
|
{
|
|
|
- if (_diOrigin.Value && _diOrigin2.Value)
|
|
|
- {
|
|
|
- EV.PostInfoLog(Module, $"Lift Pin already on original position.");
|
|
|
- sw.Stop();
|
|
|
- return true;
|
|
|
- }
|
|
|
+ //if (_diOrigin.Value && _diOrigin2.Value && _diOrigin3.Value)
|
|
|
+ //{
|
|
|
+ // EV.PostInfoLog(Module, $"Lift Pin already on original position.");
|
|
|
+ // sw.Stop();
|
|
|
+ // return true;
|
|
|
+ //}
|
|
|
_doOrigin.Value = true;
|
|
|
}
|
|
|
break;
|
|
@@ -366,15 +406,73 @@ namespace VirgoRT.Devices
|
|
|
NoDuplicatedAlarm($"Lift Pin DI-{_diCOMMAlarm.Index} alarm");
|
|
|
}
|
|
|
|
|
|
+ if(_di1_L_Servo_Driver_Alarm.Value)
|
|
|
+ {
|
|
|
+ NoDuplicatedAlarm($"Lift Pin DI-{_di1_L_Servo_Driver_Alarm.Index} alarm");
|
|
|
+ }
|
|
|
+ if (_di1_L_Servo_Driver_Warning.Value)
|
|
|
+ {
|
|
|
+ NoDuplicatedAlarm($"Lift Pin DI-{_di1_L_Servo_Driver_Warning.Index} alarm");
|
|
|
+ }
|
|
|
+ if (_di1_Search_ORG_Fail_Alarm.Value)
|
|
|
+ {
|
|
|
+ NoDuplicatedAlarm($"Lift Pin DI-{_di1_Search_ORG_Fail_Alarm.Index} alarm");
|
|
|
+ }
|
|
|
+ if (_di1_ABS_Fail_Alarm.Value)
|
|
|
+ {
|
|
|
+ NoDuplicatedAlarm($"Lift Pin DI-{_di1_ABS_Fail_Alarm.Index} alarm");
|
|
|
+ }
|
|
|
+ if (_di1_Up_Limit_Alarm.Value)
|
|
|
+ {
|
|
|
+ NoDuplicatedAlarm($"Lift Pin DI-{_di1_Up_Limit_Alarm.Index} alarm");
|
|
|
+ }
|
|
|
+ if (_di1_Down_Limit_Alarm.Value)
|
|
|
+ {
|
|
|
+ NoDuplicatedAlarm($"Lift Pin DI-{_di1_Down_Limit_Alarm.Index} alarm");
|
|
|
+ }
|
|
|
+ if (_di1_Over_Software_Alarm.Value)
|
|
|
+ {
|
|
|
+ NoDuplicatedAlarm($"Lift Pin DI-{_di1_Over_Software_Alarm.Index} alarm");
|
|
|
+ }
|
|
|
+
|
|
|
+ if (_di2_L_Servo_Driver_Alarm.Value)
|
|
|
+ {
|
|
|
+ NoDuplicatedAlarm($"Lift Pin DI-{_di2_L_Servo_Driver_Alarm.Index} alarm");
|
|
|
+ }
|
|
|
+ if (_di2_L_Servo_Driver_Warning.Value)
|
|
|
+ {
|
|
|
+ NoDuplicatedAlarm($"Lift Pin DI-{_di2_L_Servo_Driver_Warning.Index} alarm");
|
|
|
+ }
|
|
|
+ if (_di2_Search_ORG_Fail_Alarm.Value)
|
|
|
+ {
|
|
|
+ NoDuplicatedAlarm($"Lift Pin DI-{_di2_Search_ORG_Fail_Alarm.Index} alarm");
|
|
|
+ }
|
|
|
+ if (_di2_ABS_Fail_Alarm.Value)
|
|
|
+ {
|
|
|
+ NoDuplicatedAlarm($"Lift Pin DI-{_di2_ABS_Fail_Alarm.Index} alarm");
|
|
|
+ }
|
|
|
+ if (_di2_Up_Limit_Alarm.Value)
|
|
|
+ {
|
|
|
+ NoDuplicatedAlarm($"Lift Pin DI-{_di2_Up_Limit_Alarm.Index} alarm");
|
|
|
+ }
|
|
|
+ if (_di2_Down_Limit_Alarm.Value)
|
|
|
+ {
|
|
|
+ NoDuplicatedAlarm($"Lift Pin DI-{_di2_Down_Limit_Alarm.Index} alarm");
|
|
|
+ }
|
|
|
+ if (_di2_Over_Software_Alarm.Value)
|
|
|
+ {
|
|
|
+ NoDuplicatedAlarm($"Lift Pin DI-{_di2_Over_Software_Alarm.Index} alarm");
|
|
|
+ }
|
|
|
+
|
|
|
if (_currentTarget == Position.Invalid)
|
|
|
return;
|
|
|
|
|
|
if ((_currentTarget == Position.position1 && _diP1.Value) ||
|
|
|
(_currentTarget == Position.position2 && _diP2.Value) ||
|
|
|
(_currentTarget == Position.position3 && _diP3.Value) ||
|
|
|
- (_currentTarget == Position.origin && (_diOrigin.Value && _diOrigin2.Value)))
|
|
|
+ (_currentTarget == Position.origin && swOrigin.ElapsedMilliseconds > 1000 && (_diOrigin.Value && _diOrigin2.Value && _diOrigin3.Value)))
|
|
|
{
|
|
|
- EV.PostInfoLog(Module, $"Lift Pin arrive {_currentTarget}");
|
|
|
+ EV.PostInfoLog(Module, $"Lift Pin arrive {_currentTarget}, ({_diOrigin.Index})_DI_Lift_Servo_1_Servo_Origin_Complete={_diOrigin.Value} ({_diOrigin2.Index})_DI_Lift_Servo_2_Servo_Origin_Complete={_diOrigin2.Value} ({_diOrigin3.Index})_DI_Lift_Servo_Group_Go_Pos_Origin_Complete={_diOrigin3.Value} ");
|
|
|
Reset();
|
|
|
return;
|
|
|
}
|
|
@@ -389,6 +487,7 @@ namespace VirgoRT.Devices
|
|
|
{
|
|
|
_currentTarget = Position.Invalid;
|
|
|
sw.Reset();
|
|
|
+ swOrigin.Reset();
|
|
|
_doP1.Value = false;
|
|
|
_doP2.Value = false;
|
|
|
_doP3.Value = false;
|