339 lines
9.4 KiB
C#
339 lines
9.4 KiB
C#
|
using Aitex.Core.RT.Device;
|
|||
|
using Aitex.Core.RT.Event;
|
|||
|
using Aitex.Core.RT.Routine;
|
|||
|
using Aitex.Core.RT.SCCore;
|
|||
|
using MECF.Framework.Common.Equipment;
|
|||
|
using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Servo.NAIS;
|
|||
|
using System;
|
|||
|
using System.Collections.Generic;
|
|||
|
using System.Linq;
|
|||
|
using System.Text;
|
|||
|
using System.Threading.Tasks;
|
|||
|
using static SicPM.PMModule;
|
|||
|
|
|||
|
namespace SicPM.Routines
|
|||
|
{
|
|||
|
class PMServoHomeRoutine : PMBaseRoutine
|
|||
|
{
|
|||
|
enum RoutineStep
|
|||
|
{
|
|||
|
SetServoOn,
|
|||
|
SetHome,
|
|||
|
|
|||
|
SetBlockOn,
|
|||
|
|
|||
|
Delay,
|
|||
|
|
|||
|
WaitStatusCorrect,
|
|||
|
|
|||
|
SetBlockOff,
|
|||
|
SetBlockOff0
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
|
|||
|
private int _timeout;
|
|||
|
|
|||
|
private NAISServo servo;
|
|||
|
|
|||
|
public PMServoHomeRoutine(ModuleName module, PMModule pm) : base(module, pm)
|
|||
|
{
|
|||
|
Module = module.ToString();
|
|||
|
Name = "Servo Home";
|
|||
|
servo = DEVICE.GetDevice<NAISServo>($"{Module}.NAISServo");
|
|||
|
|
|||
|
_timeout = SC.GetValue<int>($"NAISServo.HomeTimeout");
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
public void Init()
|
|||
|
{
|
|||
|
//_useSettingValue = false;
|
|||
|
}
|
|||
|
public void Init(float basePressure, int ventDelayTime)
|
|||
|
{
|
|||
|
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
public override Result Start(params object[] objs)
|
|||
|
{
|
|||
|
if (PMDevice.CheckServoAlarm())
|
|||
|
{
|
|||
|
EV.PostWarningLog(Module, "can not up,confinementring is error.");
|
|||
|
return Result.FAIL;
|
|||
|
}
|
|||
|
if (PMDevice.CheckServoIsBusy())
|
|||
|
{
|
|||
|
EV.PostWarningLog(Module, "can not vent,confinementring is busy.");
|
|||
|
return Result.FAIL;
|
|||
|
}
|
|||
|
Reset();
|
|||
|
|
|||
|
Notify("Start");
|
|||
|
return Result.RUN;
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
public override Result Monitor()
|
|||
|
{
|
|||
|
if (servo == null)
|
|||
|
{
|
|||
|
EV.PostAlarmLog(Module, $"{Module} {Name}, Can Not Run Servo Home,Please Install NAISServo.");
|
|||
|
return Result.FAIL;
|
|||
|
}
|
|||
|
try
|
|||
|
{
|
|||
|
|
|||
|
|
|||
|
ServoBlockOff((int)RoutineStep.SetBlockOff0, PMDevice, _timeout);
|
|||
|
|
|||
|
ServoHome((int)RoutineStep.SetHome, PMDevice, _timeout);
|
|||
|
|
|||
|
ServoBlockOn((int)RoutineStep.SetBlockOn, PMDevice, _timeout);
|
|||
|
|
|||
|
TimeDelay((int)RoutineStep.Delay, 2);
|
|||
|
|
|||
|
WaitStatusCorrect((int)RoutineStep.WaitStatusCorrect, PMDevice, _timeout);
|
|||
|
|
|||
|
//ServoBlockOff((int)RoutineStep.SetBlockOff, PMDevice, _timeout);
|
|||
|
}
|
|||
|
catch (RoutineBreakException)
|
|||
|
{
|
|||
|
return Result.RUN;
|
|||
|
}
|
|||
|
catch (RoutineFaildException)
|
|||
|
{
|
|||
|
return Result.FAIL;
|
|||
|
}
|
|||
|
|
|||
|
Notify("Finished");
|
|||
|
|
|||
|
return Result.DONE;
|
|||
|
}
|
|||
|
|
|||
|
private void ServoHome(int id, PMModuleBase pm, int timeout)
|
|||
|
{
|
|||
|
Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
|
|||
|
{
|
|||
|
Notify($"Run {pm.Name} set Block Home.");
|
|||
|
|
|||
|
//if (!servo.SetBlockNoHigh())
|
|||
|
//{
|
|||
|
// Stop(reason);
|
|||
|
// return false;
|
|||
|
//}
|
|||
|
servo.SetBlockHome();
|
|||
|
return true;
|
|||
|
}, () =>
|
|||
|
{
|
|||
|
if (servo.AlarmStatus)
|
|||
|
{
|
|||
|
Stop($"{pm.Name} error");
|
|||
|
return null;
|
|||
|
}
|
|||
|
if (!servo.IsBusy)
|
|||
|
return true;
|
|||
|
|
|||
|
return false;
|
|||
|
|
|||
|
}, timeout * 1000);
|
|||
|
|
|||
|
if (ret.Item1)
|
|||
|
{
|
|||
|
if (ret.Item2 == Result.FAIL)
|
|||
|
{
|
|||
|
throw (new RoutineFaildException());
|
|||
|
}
|
|||
|
else if (ret.Item2 == Result.TIMEOUT) //timeout
|
|||
|
{
|
|||
|
Stop($"{pm.Name} set Block Home timeout, over {timeout} seconds");
|
|||
|
throw (new RoutineFaildException());
|
|||
|
}
|
|||
|
else
|
|||
|
throw (new RoutineBreakException());
|
|||
|
}
|
|||
|
}
|
|||
|
private void ServoBlockOn(int id, PMModuleBase pm, int timeout)
|
|||
|
{
|
|||
|
Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
|
|||
|
{
|
|||
|
Notify($"Run {pm.Name} set Stb On.");
|
|||
|
|
|||
|
//if (!servo.SetBlockNoHigh())
|
|||
|
//{
|
|||
|
// Stop(reason);
|
|||
|
// return false;
|
|||
|
//}
|
|||
|
servo.SetStbOn();
|
|||
|
return true;
|
|||
|
}, () =>
|
|||
|
{
|
|||
|
if (servo.AlarmStatus)
|
|||
|
{
|
|||
|
Stop($"{pm.Name} Servo in error State.");
|
|||
|
return null;
|
|||
|
}
|
|||
|
if (!servo.IsBusy)
|
|||
|
{
|
|||
|
PMDevice.ServoState = ServoStates.Homing;
|
|||
|
return true;
|
|||
|
}
|
|||
|
|
|||
|
return false;
|
|||
|
|
|||
|
}, timeout * 1000);
|
|||
|
|
|||
|
if (ret.Item1)
|
|||
|
{
|
|||
|
if (ret.Item2 == Result.FAIL)
|
|||
|
{
|
|||
|
throw (new RoutineFaildException());
|
|||
|
}
|
|||
|
else if (ret.Item2 == Result.TIMEOUT) //timeout
|
|||
|
{
|
|||
|
Stop($"{pm.Name} set Stb On timeout, over {timeout} seconds");
|
|||
|
throw (new RoutineFaildException());
|
|||
|
}
|
|||
|
else
|
|||
|
throw (new RoutineBreakException());
|
|||
|
}
|
|||
|
}
|
|||
|
private void ServoBlockOff(int id, PMModuleBase pm, int timeout)
|
|||
|
{
|
|||
|
Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
|
|||
|
{
|
|||
|
Notify($"Run {pm.Name} set Stb Off.");
|
|||
|
|
|||
|
//if (!servo.SetBlockNoHigh())
|
|||
|
//{
|
|||
|
// Stop(reason);
|
|||
|
// return false;
|
|||
|
//}
|
|||
|
servo.SetStbOff();
|
|||
|
return true;
|
|||
|
}, () =>
|
|||
|
{
|
|||
|
if (servo.AlarmStatus)
|
|||
|
{
|
|||
|
Stop($"{pm.Name} Servo in error State");
|
|||
|
return null;
|
|||
|
}
|
|||
|
if (!servo.IsBusy)
|
|||
|
return true;
|
|||
|
|
|||
|
return false;
|
|||
|
|
|||
|
}, timeout * 1000);
|
|||
|
|
|||
|
if (ret.Item1)
|
|||
|
{
|
|||
|
if (ret.Item2 == Result.FAIL)
|
|||
|
{
|
|||
|
throw (new RoutineFaildException());
|
|||
|
}
|
|||
|
else if (ret.Item2 == Result.TIMEOUT) //timeout
|
|||
|
{
|
|||
|
Stop($"{pm.Name} set Stb Off timeout, over {timeout} seconds");
|
|||
|
throw (new RoutineFaildException());
|
|||
|
}
|
|||
|
else
|
|||
|
throw (new RoutineBreakException());
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
private void SetServoOn(int id, PMModuleBase pm, int timeout)
|
|||
|
{
|
|||
|
Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
|
|||
|
{
|
|||
|
Notify($"Run {pm.Name} set Servo On.");
|
|||
|
servo.SetServoOn();
|
|||
|
return true;
|
|||
|
}, () =>
|
|||
|
{
|
|||
|
if (servo.AlarmStatus)
|
|||
|
{
|
|||
|
Stop($"{pm.Name} Servo in error State");
|
|||
|
return null;
|
|||
|
}
|
|||
|
if (!servo.IsBusy)
|
|||
|
return true;
|
|||
|
|
|||
|
return false;
|
|||
|
|
|||
|
}, timeout * 1000);
|
|||
|
|
|||
|
if (ret.Item1)
|
|||
|
{
|
|||
|
if (ret.Item2 == Result.FAIL)
|
|||
|
{
|
|||
|
throw (new RoutineFaildException());
|
|||
|
}
|
|||
|
else if (ret.Item2 == Result.TIMEOUT) //timeout
|
|||
|
{
|
|||
|
Stop($"{pm.Name} set Servo On timeout, over {timeout} seconds");
|
|||
|
throw (new RoutineFaildException());
|
|||
|
}
|
|||
|
else
|
|||
|
throw (new RoutineBreakException());
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
private void WaitStatusCorrect(int id, PMModule pm, int timeout)
|
|||
|
{
|
|||
|
Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
|
|||
|
{
|
|||
|
Notify($"Run {pm.Name} Wait, Wait Status Correct");
|
|||
|
servo.Query();
|
|||
|
return true;
|
|||
|
}, () =>
|
|||
|
{
|
|||
|
if (servo.AlarmStatus)
|
|||
|
{
|
|||
|
Stop($"{pm.Name} Servo in error State");
|
|||
|
return null;
|
|||
|
}
|
|||
|
if (!servo.IsBusy)
|
|||
|
{
|
|||
|
if (servo.IsArrived)
|
|||
|
{
|
|||
|
PMDevice.ServoState = ServoStates.Home;
|
|||
|
return true;
|
|||
|
}
|
|||
|
else
|
|||
|
{
|
|||
|
servo.QueryArrival();
|
|||
|
}
|
|||
|
}
|
|||
|
return false;
|
|||
|
|
|||
|
}, timeout * 1000);
|
|||
|
|
|||
|
if (ret.Item1)
|
|||
|
{
|
|||
|
if (ret.Item2 == Result.FAIL)
|
|||
|
{
|
|||
|
throw (new RoutineFaildException());
|
|||
|
}
|
|||
|
else if (ret.Item2 == Result.TIMEOUT) //timeout
|
|||
|
{
|
|||
|
Stop($"{pm.Name} Wait Status Correct timeout, over {timeout} seconds");
|
|||
|
throw (new RoutineFaildException());
|
|||
|
}
|
|||
|
else
|
|||
|
throw (new RoutineBreakException());
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
public override void Abort()
|
|||
|
{
|
|||
|
if (!PMDevice.StopVent(out string reason))
|
|||
|
{
|
|||
|
EV.PostWarningLog(Module, reason);
|
|||
|
}
|
|||
|
|
|||
|
Stop("aborted");
|
|||
|
}
|
|||
|
}
|
|||
|
}
|