1. 增加深浦深度传感器驱动

2. 增加CheckArmHeightRoutine用于检查手臂深度，相关站点设置StageAStationNumber,相关界面在TMTransfer界面QueryHeight按钮，数据将通过LOG反馈

FrameworkLocal/RTEquipmentLibrary/HardwareUnits/Robots/HongHu/HongHuVR.cs

@@ -174,6 +174,7 @@ namespace MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.HongHu
 
             _StationNumbers[ModuleName.CoolingBuffer1] = SC.GetStringValue("TM.CoolingBuffer1LowerStationNumber");
             _StationNumbers[ModuleName.CoolingBuffer2] = SC.GetStringValue("TM.CoolingBuffer2LowerStationNumber");
+            _StationNumbers[ModuleName.StageA] = SC.GetStringValue("TM.HeightCheckStationNumber");
 
             Cool1UpperStationNumber = SC.GetStringValue("TM.CoolingBuffer1UpperStationNumber");
             Cool1LowerStationNumber = SC.GetStringValue("TM.CoolingBuffer1LowerStationNumber");

FrameworkLocal/RTEquipmentLibrary/HardwareUnits/SerialPortSensor/ShenPuHeightSensor.cs

@@ -0,0 +1,103 @@
+using Aitex.Core.RT.Event;
+using Aitex.Core.RT.Routine;
+using Aitex.Core.RT.SCCore;
+using Aitex.Core.Util;
+using MECF.Framework.Common.Communications;
+using MECF.Framework.Common.Equipment;
+using System;
+using System.Collections.Generic;
+using System.IO.Ports;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace MECF.Framework.RT.EquipmentLibrary.HardwareUnits.SerialPortSensor
+{
+    public class ShenPuHeightSensor
+    {
+        #region 私有变量
+        private AsyncSerialPort _serialport;
+        private ModuleName _module;
+        private bool _IsAsciiMode;
+        private string _portname;
+        private RState _status;
+        //private PeriodicJob _thread;
+        private object _locker = new object();
+        private bool _IsNewHeight = false;
+
+        //replay 01 03 04 |00 00 13 88|(1/k mm) (xx xx)
+        private byte[] QueryMsg = new byte[] { 0x01, 0x03, 0x00, 0x01, 0x00, 0x02, 0x95, 0xCB };
+        #endregion
+
+        #region 公开变量
+
+        public double CurrentHeight = -1;
+        public bool IsNewHeight => _IsNewHeight;
+
+        #endregion
+
+        #region 构造函数
+        public ShenPuHeightSensor(ModuleName module)
+        {
+            _module = module;
+            _IsAsciiMode = false;
+            _portname = SC.GetStringValue($"{_module}.HeightSensorPort");
+            _serialport = new AsyncSerialPort(_portname, 9600, 8, Parity.None, StopBits.One, "", _IsAsciiMode);
+            _serialport.Open();
+            _status = RState.End;
+            //_serialport.OnDataChanged += onDataChange;
+            _serialport.OnBinaryDataChanged += onBinaryChange;
+            //_thread = new PeriodicJob(50, fnTimer, _module.ToString(), true);
+        }
+
+        #endregion
+
+        #region 公开方法
+
+        public bool QueryHeight()
+        {
+            _IsNewHeight = true;
+            CurrentHeight = -1;
+            return _serialport.Write(QueryMsg);
+        }
+
+        #endregion
+
+        #region 私有方法
+
+
+        private void onBinaryChange(byte[] msgs)
+        {
+            //校验头消息 分散头 消息
+            lock (_locker)
+            {
+                List<byte> _msgsAsList= msgs.ToList();
+                //舍去前段 只需要最后的一段保证时效性
+                for (int idx_last = _msgsAsList.Count -3; idx_last > 3; idx_last -- )
+                {
+                    if (_msgsAsList[idx_last] == 0x04 && _msgsAsList[idx_last - 1] == 0x03 && _msgsAsList[idx_last - 2] == 0x01 && idx_last + 6 < _msgsAsList.Count -1 )
+                    {
+                        byte[] _height_hex = new byte[] { _msgsAsList[idx_last + 1], _msgsAsList[idx_last + 2], _msgsAsList[idx_last + 3], _msgsAsList[idx_last + 4] };
+                        _IsNewHeight = true;
+                        CurrentHeight = Convert.ToInt32(_height_hex) * 0.0001;
+                        break;
+                    }
+                }
+
+                if (!_IsNewHeight)
+                {
+                    string HexMsg = "";
+                    _msgsAsList.ForEach(x=> HexMsg += x.ToString());
+                    EV.PostAlarmLog(_module.ToString(), $"未收到高度信息， msg:{HexMsg}");
+                }
+            }
+        }
+
+        private void onDataChange(string obj)
+        {
+
+        }
+        
+        #endregion
+    }
+}

FrameworkLocal/RTEquipmentLibrary/RTEquipmentLibrary.csproj

@@ -158,6 +158,7 @@
     <Compile Include="HardwareUnits\Robots\YaskawaRobots\YaskawaRobot.cs" />
     <Compile Include="HardwareUnits\Robots\YaskawaRobots\YaskawaRobotConnection.cs" />
     <Compile Include="HardwareUnits\Robots\YaskawaRobots\YaskawaRobotHandler.cs" />
+    <Compile Include="HardwareUnits\SerialPortSensor\ShenPuHeightSensor.cs" />
     <Compile Include="HardwareUnits\SMIFs\Brooks\BrooksSMIF.cs" />
     <Compile Include="HardwareUnits\SMIFs\Brooks\BrooksSMIFConnection.cs" />
     <Compile Include="HardwareUnits\SMIFs\Brooks\BrooksSMIFHandler.cs" />

FrameworkLocal/RTModuleLibrary/TMModules/TMEntityBase.cs

@@ -42,6 +42,7 @@ namespace MECF.Framework.RT.ModuleLibrary.TMModules
             SetSpeeding,
             SaveSpeeding,
             QuerySpeeding,
+            QueryHeight,
         }
 
         public enum MSG
@@ -78,6 +79,7 @@ namespace MECF.Framework.RT.ModuleLibrary.TMModules
             SetSpeed,
             SaveSpeed,
             QuerySpeed,
+            CheckHeight,
 
         }
         

Mars/EfemDualRT/Config/System.sccfg

@@ -261,6 +261,8 @@
     <config default="60" name="HomeTimeout" nameView="Home Timeout" description="TM初始化超时" max="300" min="1" paramter="" tag="" unit="s" type="Integer" />
     <config default="60" name="MotionTimeout" nameView="Motion Timeout" description="motion time out" max="600" min="1" paramter="" tag="" unit="s" type="Integer" />
     <config default="192.168.10.3:1102" name="IPAddress" nameView="IP Address" description="TM Robot IP、端口设置;default 10.0.0.100:13000" max="" min="" paramter="" tag="" unit="" type="String" />
+    <config default="false" name="HasHeightSensor"  description="是否配有检测手臂深度传感器" max="0" min="0" paramter="" tag="" unit="" type="Bool"  />
+    <config default="COM11" name="HeightSensorPort" nameView="Height Sensor Port" description="深度传感器Port号" max="" min="" paramter="" tag="" unit="" type="String" />
     <config default="03" name="PMAStationNumber" nameView="PMA Station Number" description="PMA Station Number" max="99" min="0" paramter="" tag="" unit="" type="String" />
     <config default="04" name="PMBStationNumber" nameView="PMB Station Number" description="PMB Station Number" max="99" min="0" paramter="" tag="" unit="" type="String" />
     <config default="05" name="PMCStationNumber" nameView="PMC Station Number" description="PMC Station Number" max="99" min="0" paramter="" tag="" unit="" type="String" />
@@ -269,6 +271,7 @@
     <config default="18" name="PMFStationNumber" nameView="PMF Station Number" description="PMF Station Number" max="99" min="0" paramter="" tag="" unit="" type="String" />
     <config default="02" name="VCEAStationNumber" nameView="VCEA Station Number" description="VCEA Station Number" max="99" min="0" paramter="" tag="" unit="" type="String" />
     <config default="01" name="VCEBStationNumber" nameView="VCEB Station Number" description="VCEB Station Number" max="99" min="0" paramter="" tag="" unit="" type="String" />
+    <config default="11" name="HeightCheckStationNumber" nameView="HeightCheck Station Number" description="检查手臂高度用站点" max="99" min="0" paramter="" tag="" unit="" type="String" />
     <config default="1" name="VPAStationNumber" nameView="VPA Station Number" description="VPA Station Number" max="99" min="0" paramter="" tag="" unit="" type="String" />
     <config default="10" name="CheckLoadStationNumber" nameView="CheckLoad Station Number" description="CheckLoad Station Number" max="99" min="0" paramter="" tag="" unit="" type="String" />
     <config default="07" name="CoolingBuffer1UpperStationNumber" nameView="CoolingBuffer1 Upper Station Number" description="CoolingBuffer1 Upper Station Number" max="99" min="0" paramter="" tag="" unit="" type="String" />

Mars/EfemDualUI/Views/Platforms/TMs/TMTransferView.xaml

@@ -142,6 +142,7 @@
                         <RowDefinition/>
                         <RowDefinition/>
                         <RowDefinition/>
+                        <RowDefinition/>
                     </Grid.RowDefinitions>
                     <Grid.ColumnDefinitions>
                         <ColumnDefinition/>
@@ -273,6 +274,14 @@
                         </i:EventTrigger>
                     </i:Interaction.Triggers>
                 </Button>
+                <Button Grid.Row="10" Grid.Column="3"  Margin="5" Height="25" Content="QueryHeight">
+                    <i:Interaction.Triggers>
+                        <i:EventTrigger EventName="Click">
+                            <cal:ActionMessage MethodName="OnQueryHeight">
+                            </cal:ActionMessage>
+                        </i:EventTrigger>
+                    </i:Interaction.Triggers>
+                </Button>
             </Grid>
             </Grid>
     </Grid>

Mars/EfemDualUI/Views/Platforms/TMs/TMTransferViewModel.cs

@@ -532,11 +532,17 @@ namespace EfemDualUI.Views.Platforms.TMs
         {
             InvokeClient.Instance.Service.DoOperation($"VTM.QuerySpeed", "NOWAFER");
         }
+
+        public void OnQueryHeight()
+        {
+            InvokeClient.Instance.Service.DoOperation($"VTM.CheckHeight");
+        }
         #endregion
 
         private DelegateCommand<object> _ModuleChangeCommand;
         public DelegateCommand<object> ModuleChangeCommand =>
             _ModuleChangeCommand ?? (_ModuleChangeCommand = new DelegateCommand<object>(OnModuleChange));
+       
 
         #region 私有方法
         //模块选择根据obj选择下拉框内容

Mars/JetMainframe/JetMainframe.csproj

@@ -60,6 +60,7 @@
     <Compile Include="TMs\TMPlaceRoutine.cs" />
     <Compile Include="TMs\TMPumpRoutine.cs" />
     <Compile Include="TMs\TMVentRoutine.cs" />
+    <Compile Include="TMs\TM\CheckArmHeightRoutine.cs" />
     <Compile Include="TMs\TM\MFExtendRoutine.cs" />
     <Compile Include="TMs\TM\MFHomeRoutine.cs" />
     <Compile Include="TMs\TM\MFLeakCheckRoutine.cs" />

Mars/JetMainframe/TMs/TM/CheckArmHeightRoutine.cs

@@ -0,0 +1,119 @@
+using Aitex.Core.RT.Event;
+using Aitex.Core.RT.Routine;
+using Aitex.Sorter.Common;
+using JetMainframe.Devices;
+using MECF.Framework.Common.Equipment;
+using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.SerialPortSensor;
+using MECF.Framework.RT.EquipmentLibrary.LogicUnits;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace JetMainframe.TMs.TM
+{
+    public class CheckArmHeightRoutine : ModuleRoutineBase, IStepRoutine
+    {
+        public enum CheckArmHeightStep
+        {
+            Blade1Goto,
+            WaitForBlade1Stable,
+            CalcuteBlade1,
+            Blade2Goto,
+            WaitForBlade2Stable,
+            CalcuteBlade2,
+            EndDelay,
+        }
+
+        private ShenPuHeightSensor _heightSensor;
+        private ITransferRobot _robot;
+        private int GotoTimeOut = 10 * 1000;//Goto动作时间
+        private int QueryTimeOut = 5 * 1000;//检查高度时间
+
+
+        public CheckArmHeightRoutine(ModuleName module, ITransferRobot robot, ShenPuHeightSensor heightSensor) : base(module.ToString())
+        {
+            _robot = robot;
+            _heightSensor = heightSensor;
+            Name = "Check Arm";
+        }
+
+
+        public RState Start(params object[] objs)
+        {
+            Reset();
+            return Runner.Start(Module, Name);
+        }
+
+
+        public RState Monitor()
+        {
+            Runner.Run(CheckArmHeightStep.Blade1Goto, Blade1Goto, CheckRobotDone, GotoTimeOut)
+                  .Delay(CheckArmHeightStep.WaitForBlade1Stable, 1000)
+                  .Run(CheckArmHeightStep.CalcuteBlade1, QuerySensor, CheckSensor1Done, QueryTimeOut)
+                  .Run(CheckArmHeightStep.Blade2Goto, Blade2Goto, CheckRobotDone, GotoTimeOut)
+                  .Delay(CheckArmHeightStep.WaitForBlade2Stable, 1000)
+                  .Run(CheckArmHeightStep.CalcuteBlade2, QuerySensor, CheckSensor2Done, QueryTimeOut)
+                  .End(CheckArmHeightStep.EndDelay, NullFun, 500);
+
+            return Runner.Status;
+        }
+
+        private bool Blade1Goto()
+        {
+            return _robot.Goto(ModuleName.StageA, Hand.Blade1,0,out _);
+        }
+
+        private bool Blade2Goto()
+        {
+            return _robot.Goto(ModuleName.StageA, Hand.Blade2, 0, out _);
+        }
+
+        private bool CheckSensor1Done()
+        {
+            if (_heightSensor.IsNewHeight)
+            {
+                EV.PostInfoLog(Module, $"获取Blade1高度:{_heightSensor.CurrentHeight} mm");
+            }
+            return _heightSensor.IsNewHeight;
+        }
+
+        private bool CheckSensor2Done()
+        {
+            if (_heightSensor.IsNewHeight)
+            {
+                EV.PostInfoLog(Module, $"获取Blade2高度:{_heightSensor.CurrentHeight} mm");
+            }
+            return _heightSensor.IsNewHeight;
+        }
+
+        private bool QuerySensor()
+        {
+            return _heightSensor.QueryHeight() && !_heightSensor.IsNewHeight;
+        }
+
+        private bool CheckRobotDone()
+        {
+            if (_robot.Status == RState.Running)
+            {
+                return false;
+            }
+            else if (_robot.Status == RState.End)
+            {
+                return true;
+            }
+            else
+            {
+                Runner.Stop($"TM Robot Goto failed, {_robot.Status}");
+                return true;
+            }
+        }
+
+
+        public void Abort()
+        {
+
+        }
+    }
+}

Mars/JetMainframe/TMs/TM/TMEntity.cs

@@ -13,6 +13,7 @@ using MECF.Framework.Common.CommonData;
 using MECF.Framework.Common.DBCore;
 using MECF.Framework.Common.Equipment;
 using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Robots.HongHu;
+using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.SerialPortSensor;
 using MECF.Framework.RT.EquipmentLibrary.LogicUnits;
 using MECF.Framework.RT.ModuleLibrary.Commons;
 using MECF.Framework.RT.ModuleLibrary.SystemModules;
@@ -86,6 +87,7 @@ namespace JetMainframe.TMs.TM
         #region 私有变量
         private readonly HongHuTM _tm;
         private readonly ITransferRobot _robot;
+        private readonly ShenPuHeightSensor _HeightSensor;
 
         private readonly MFHomeRoutine _homeRoutine;
         private readonly MFPickRoutine _pickRoutine;
@@ -99,6 +101,7 @@ namespace JetMainframe.TMs.TM
         private readonly MFRetractRoutine _retractRoutine;
         private readonly MFExtendRoutine _extendRoutine;
         private readonly MFLeakCheckRoutine _LeakCheckRoutine;
+        private readonly CheckArmHeightRoutine _CheckArmHeightRoutine;
 
 
         //private readonly
@@ -119,6 +122,12 @@ namespace JetMainframe.TMs.TM
             _robotWatch = new Stopwatch();
             _robot.AWCErrorHandler += NeedAWC;
 
+            if (SC.GetValue<bool>("TM.HasHeightSensor"))
+            {
+                _HeightSensor = new ShenPuHeightSensor(ModuleName.TM);
+                _CheckArmHeightRoutine = new CheckArmHeightRoutine(module, _robot, _HeightSensor);
+            }
+
             _homeRoutine = new MFHomeRoutine(module, _tm, _robot);
             _pickRoutine = new MFPickRoutine(module, _tm, _robot);
             _placeRoutine = new MFPlaceRoutine(module, _tm, _robot);
@@ -132,7 +141,6 @@ namespace JetMainframe.TMs.TM
             _retractRoutine = new MFRetractRoutine(module, _tm, _robot);
             _LeakCheckRoutine = new MFLeakCheckRoutine(module, _tm);
 
-
             IsOnline = false;
 
             InitFsmMap();
@@ -203,6 +211,7 @@ namespace JetMainframe.TMs.TM
             OP.Subscribe("VTM.SetSpeed", (cmd, args) => { PostMsg(MSG.SetSpeed, args); return true; });
             OP.Subscribe("VTM.QuerySpeed", (cmd, args) => { PostMsg(MSG.QuerySpeed, args); return true; });
             OP.Subscribe("VTM.SaveSpeed", (cmd, args) => { PostMsg(MSG.SaveSpeed, args); return true; });
+            OP.Subscribe("VTM.CheckHeight", (cmd, args) => { PostMsg(MSG.CheckHeight, args); return true; });
             return true;
         }
 
@@ -297,6 +306,11 @@ namespace JetMainframe.TMs.TM
             Transition(STATE.SaveSpeeding, FSM_MSG.TIMER, fnSaveSpeedTimeout, STATE.Idle);
             Transition(STATE.SaveSpeeding, MSG.Abort, fnAbortSaveSpeed, STATE.Idle);
 
+            //Check Blade Height
+            Transition(STATE.Idle, MSG.CheckHeight, fnStartCheckHeight, STATE.QueryHeight);
+            Transition(STATE.QueryHeight, FSM_MSG.TIMER, fnCheckHeightTimeout, STATE.Idle);
+            Transition(STATE.QueryHeight, MSG.Abort, fnAbortCheckHeight, STATE.Idle);
+
             //Control
             AnyStateTransition(FSM_MSG.TIMER, fnControlPressure, FSM_STATE.SAME);
 
@@ -668,6 +682,33 @@ namespace JetMainframe.TMs.TM
             return _robot.QuerySpeed(param[0].ToString());
         }
 
+        private bool fnStartCheckHeight(object[] param)
+        {
+            if (_CheckArmHeightRoutine == null)
+                return false;
+            else
+                return _CheckArmHeightRoutine.Start() == RState.Running;
+            
+        }
+
+        private bool fnCheckHeightTimeout(object[] param)
+        {
+            RState ret = _CheckArmHeightRoutine.Monitor();
+            if (ret == RState.Failed || ret == RState.Timeout)
+            {
+                PostMsg(MSG.Error);
+                return false;
+            }
+
+            return ret == RState.End;
+        }
+
+        private bool fnAbortCheckHeight(object[] param)
+        {
+            _CheckArmHeightRoutine.Abort();
+            return true;
+        }
+
         private bool fnSetSpeedTimeout(object[] param)
         {
             RState ret = _robot.Status;