123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230 |
- using Aitex.Core.RT.Device;
- using Aitex.Core.RT.Device.Unit;
- using Aitex.Core.RT.Event;
- using Aitex.Core.RT.Routine;
- using Aitex.Core.RT.SCCore;
- using Aitex.Core.Util;
- using Aitex.Sorter.Common;
- using MECF.Framework.Common.Alarms;
- using MECF.Framework.Common.Device.Bases;
- using MECF.Framework.Common.Equipment;
- using MECF.Framework.Common.SubstrateTrackings;
- using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robot;
- using System;
- using System.Collections.Generic;
- using System.Linq;
- using System.Text;
- using System.Threading.Tasks;
- using FurnaceRT.Equipments.Systems;
- using static Aitex.Core.RT.Device.Unit.IoBoat;
- using FurnaceRT.Devices;
- namespace FurnaceRT.Equipments.Boats
- {
- public class BoatZAxisManualMove : ModuleRoutine, IRoutine
- {
- enum RoutineStep
- {
- AutoShutterOpen,
- AutoShutterClose,
- AutoShutterUp,
- AutoShutterDown,
- SetBoatSpeed,
- BoatZAxisMove,
- CheckPrepareMove,
- }
- private BoatPosition _targetPosition;
- private float _speed;
- private BoatModule _boatModule;
- private int _timeout = 0;
- private int _shutterTimeout = 0;
- private IoSensor _sensorBEHumanInterlock;
- private IoSensor _sensorR5ArmNotExtendToBE;
- private IoSensor _sensorASOpenStatus;
- private IoSensor _sensorASTiltStatus;
- public BoatZAxisManualMove(BoatModule boatModule)
- {
- _boatModule = boatModule;
- Module = boatModule.Module;
- Name = "ZAxis Manual Move";
- }
- public void Init(string targetPosition, string speed)
- {
- _targetPosition = (BoatPosition)Enum.Parse(typeof(BoatPosition), targetPosition);
- float.TryParse(speed, out _speed);
- var para = new List<object> { _targetPosition, _speed };
- _boatModule.BoatZAxisMoveFailedForInterlock.RetryMessage = (int)BoatModule.MSG.ZAxisManualMoveRetry;
- _boatModule.BoatZAxisMoveFailedForInterlock.RetryMessageParas = para.ToArray();
- _boatModule.BoatZAxisMoveTimeOut.RetryMessage = (int)BoatModule.MSG.ZAxisManualMoveRetry;
- _boatModule.BoatZAxisMoveTimeOut.RetryMessageParas = para.ToArray();
- }
- public Result Start(params object[] objs)
- {
- Reset();
- string reason = string.Empty;
- if (_targetPosition == BoatPosition.None)
- return Result.DONE;
- _timeout = SC.GetValue<int>($"{Module}.MotionTimeout");
- _shutterTimeout = SC.GetValue<int>($"{Module}.AutoShutter.MotionTimeout");
- if (!_boatModule.CheckPrepareMove(out reason, false))
- {
- _boatModule.BoatZAxisMoveFailedForInterlock.Set(reason);
- return Result.FAIL;
- }
- Notify($"Start");
- return Result.RUN;
- }
- public void Abort()
- {
- _boatModule.BoatZAxisStop();
- }
- public override Result Monitor()
- {
- try
- {
- PauseRountine(_boatModule.RAxisDevice.IsPause);
- if (_boatModule.RAxisDevice.IsPause)
- return Result.RUN;
- if (_boatModule.ZAxisDevice.IsError)
- return Result.FAIL;
- AutoShutterOpen((int)RoutineStep.AutoShutterOpen, true, _shutterTimeout);
- CheckPrepareMove((int)RoutineStep.CheckPrepareMove, 2);
- SetBoatZAxisMove((int)RoutineStep.BoatZAxisMove, _targetPosition, _speed, _timeout);
- }
- catch (RoutineBreakException)
- {
- return Result.RUN;
- }
- catch (RoutineFaildException ex)
- {
- return Result.FAIL;
- }
- Notify("Finished");
- return Result.DONE;
- }
- private void AutoShutterOpen(int id, bool isOpen, int timeout)
- {
- Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
- {
- Notify($"shutter {(isOpen ? "open" : "close")}");
- string reason;
- if (!_boatModule.ShutterDevice.SetOpen(isOpen, out reason))
- {
- _boatModule.ShutterDevice.AutoShutterMoveFailedForInterlock.Set();
- return false;
- }
- return true;
- }, () =>
- {
- return isOpen ? _boatModule.ShutterDevice.OpenCloseStatus == DeviceStatus.Open : _boatModule.ShutterDevice.OpenCloseStatus == DeviceStatus.Close;
- }, timeout * 1000);
- if (ret.Item1)
- {
- if (ret.Item2 == Result.FAIL)
- {
- throw (new RoutineFaildException());
- }
- else if (ret.Item2 == Result.TIMEOUT) //timeout
- {
- if (isOpen)
- {
- _boatModule.ShutterDevice.AutoShutterOpenTimeOut.Set($"can not complete in {timeout} seconds");
- }
- else
- {
- _boatModule.ShutterDevice.AutoShutterCloseTimeOut.Set($"can not complete in {timeout} seconds");
- }
- throw (new RoutineFaildException());
- }
- else
- throw (new RoutineBreakException());
- }
- }
- private void CheckPrepareMove(int id, int timeout)
- {
- var reason = "";
- Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
- {
- Notify($"Check boat move enable");
- return true;
- }, () =>
- {
- return _boatModule.CheckPrepareMove(out reason);
- }, timeout * 2 * 1000);
- if (ret.Item1)
- {
- if (ret.Item2 == Result.FAIL)
- {
- throw (new RoutineFaildException());
- }
- else if (ret.Item2 == Result.TIMEOUT) //timeout
- {
- _boatModule.BoatZAxisMoveFailedForInterlock.Set($"{reason} is not OK, can not complete in {timeout} seconds");
- throw (new RoutineFaildException());
- }
- else
- throw (new RoutineBreakException());
- }
- }
- private void SetBoatZAxisMove(int id, BoatPosition position, float speed, int timeout)
- {
- Tuple<bool, Result> ret = ExecuteAndWait(id, () =>
- {
- Notify($"Boat ZAxis movet to {position}");
- string reason;
- if (!_boatModule.ZAxisDevice.SetServoMoveTo(position.ToString(), out reason, speed))
- {
- //_boatModule.BoatDevice.BoatRAxisMoveFailedForInterlock.Description = reason;
- _boatModule.BoatZAxisMoveFailedForInterlock.Set(reason);
- }
- return true;
- }, () =>
- {
- if (_boatModule.ZAxisDevice.IsError)
- return null;
- return _boatModule.ZAxisDevice.CheckServoAtPosition(position.ToString());
- }, timeout * 2 * 1000);
- if (ret.Item1)
- {
- if (ret.Item2 == Result.FAIL)
- {
- _boatModule.ZAxisDevice.ServoStop();
- throw (new RoutineFaildException());
- }
- else if (ret.Item2 == Result.TIMEOUT) //timeout
- {
- _boatModule.ZAxisDevice.ServoStop();
- _boatModule.BoatZAxisMoveTimeOut.Set($"can not complete in {timeout} seconds");
- throw (new RoutineFaildException());
- }
- else
- throw (new RoutineBreakException());
- }
- }
- }
- }
|