Add GalilRotationAxis; Modify SRD related configs；

CyberX8_RT/Config/Devices/AxisProviderCfg.xml

@@ -122,46 +122,26 @@
     <JogLimit>5</JogLimit>
 	<MinMoveTime>0.5</MinMoveTime>
 	<MotorPositionRate>1</MotorPositionRate>
-  </Axis>
-  <Axis Name="SRD1.Arm">
-    <Driver>Beckhoff</Driver>
-    <!-- <Driver>Sim</Driver> -->
-    <Units>deg</Units>
-	<!-- 512 X 4 counts/rev * 128:1 gearbox * 3:1 gear, 786432 counts/rev * 1/360 rev/deg = 2184.533333 counts/degree -->
-    <ScaleFactor>2184.533333</ScaleFactor>
-    <JogLimit>5</JogLimit>
-	<MinMoveTime>0.5</MinMoveTime>
-	<MotorPositionRate>1</MotorPositionRate>
-  </Axis>
+  </Axis>  
   <Axis Name="SRD1.Rotation">
     <Driver>Beckhoff</Driver>
     <!-- <Driver>Sim</Driver> -->
     <Units>deg</Units>
 	<!-- 5294 X 4 counts/rev * 1/360 rev/deg = 58.822222 counts/deg -->
-    <ScaleFactor>58.822222</ScaleFactor>
+    <ScaleFactor>91</ScaleFactor>
     <JogLimit>5</JogLimit>
     <!--Units>rev</Units-->
 	<!-- 5294 X 4 counts/rev = 21176 counts/rev -->
     <!--ScaleFactor>21176</ScaleFactor-->
 	<MinMoveTime>0.5</MinMoveTime>
 	<MotorPositionRate>1</MotorPositionRate>
-  </Axis>
-  <Axis Name="SRD2.Arm">
-    <Driver>Beckhoff</Driver>
-    <!-- <Driver>Sim</Driver> -->
-    <Units>deg</Units>
-    <JogLimit>5</JogLimit>
-	<!-- 512 X 4 counts/rev * 128:1 gearbox * 3:1 gear, 786432 counts/rev * 1/360 rev/deg = 2184.533333 counts/degree -->
-    <ScaleFactor>2184.533333</ScaleFactor>
-	<MinMoveTime>0.5</MinMoveTime>
-	<MotorPositionRate>1</MotorPositionRate>
-  </Axis>
+  </Axis>  
   <Axis Name="SRD2.Rotation">
     <Driver>Beckhoff</Driver>
     <!-- <Driver>Sim</Driver> -->
     <Units>deg</Units>
 	<!-- 5294 X 4 counts/rev * 1/360 rev/deg = 58.822222 counts/deg -->
-    <ScaleFactor>58.822222</ScaleFactor>
+    <ScaleFactor>91</ScaleFactor>
     <JogLimit>5</JogLimit>
     <!--Units>rev</Units-->
 	<!-- 5294 X 4 counts/rev = 21176 counts/rev -->

CyberX8_RT/Config/Devices/GalilControllerCfg.xml

@@ -196,4 +196,32 @@
 			<PositiveTorqueLimit>20</PositiveTorqueLimit>
 		</GalilAxisConfig>
   </GalilDeviceConfig>
+	<GalilDeviceConfig Module="SRD1" IpAddress="10.0.0.24" Port="58678" SendTimeout="2000" RecvTimeout="2000" GalilType="Galil21">
+		<Dig_In>
+			<DI Name="DI0"  Address="0" Invert="false"/>
+			<DI Name="DI1"  Address="1" Invert="false"/>
+			<DI Name="DI2"  Address="2" Invert="false"/>
+			<DI Name="DI3"  Address="3" Invert="false"/>
+			<DI Name="D14"  Address="4" Invert="false"/>
+			<DI Name="DI5"  Address="5" Invert="false"/>
+			<DI Name="DI6"  Address="6" Invert="false"/>
+			<DI Name="DI7"  Address="7" Invert="false"/>
+			<DI Name="DI8"  Address="8" Invert="false"/>
+			<DI Name="DI9"  Address="9" Invert="false"/>
+		</Dig_In>
+		<GalilAxisConfig Name="Rotation" Index="0" Type="GalilRotation" CNType="-1">
+			<Speed>529400</Speed>
+			<Acceleration>84000</Acceleration>
+			<Deceleration>84000</Deceleration>
+			<HomingSpeed>2500</HomingSpeed>
+			<HomingOffset>0</HomingOffset>
+			<HomingTimeOut>60000</HomingTimeOut>
+			<HomingAcceleration>84000</HomingAcceleration>
+			<HomingDeceleration>420000</HomingDeceleration>
+			<ReverseSoftwareLimit>0</ReverseSoftwareLimit>
+			<ForwardSoftwareLimit>0</ForwardSoftwareLimit>
+			<NegativeTorqueLimit>130</NegativeTorqueLimit>
+			<PositiveTorqueLimit>20</PositiveTorqueLimit>
+		</GalilAxisConfig>
+	</GalilDeviceConfig>	
 </GalilControllerConfig>

CyberX8_RT/CyberX8_RT.csproj

@@ -168,6 +168,12 @@
     <Compile Include="Devices\AXIS\GalilLipsel\GalilLipselStopPositionRoutine.cs" />
     <Compile Include="Devices\AXIS\GalilLipsel\GalilLipselSwitchOffRoutine.cs" />
     <Compile Include="Devices\AXIS\GalilLipsel\GalilLipselSwitchOnRoutine.cs" />
+    <Compile Include="Devices\AXIS\GalilRotation\GalilRotationHomeRoutine.cs" />
+    <Compile Include="Devices\AXIS\GalilRotation\GalilRotationAxis.cs" />
+    <Compile Include="Devices\AXIS\GalilRotation\GalilRotationProfilePositionRoutine.cs" />
+    <Compile Include="Devices\AXIS\GalilRotation\GalilRotationStopPositionRoutine.cs" />
+    <Compile Include="Devices\AXIS\GalilRotation\GalilRotationSwitchOffRoutine.cs" />
+    <Compile Include="Devices\AXIS\GalilRotation\GalilRotationSwitchOnRoutine.cs" />
     <Compile Include="Devices\AXIS\Galil\GalilAxis.cs" />
     <Compile Include="Devices\AXIS\Galil\GalilHomeRoutine.cs" />
     <Compile Include="Devices\AXIS\Galil\GalilProfilePositionRoutine.cs" />

CyberX8_RT/Devices/AXIS/AxisManager.cs

@@ -40,6 +40,8 @@ namespace CyberX8_RT.Devices.AXIS
                     return new GalilAxis(module, name);
                 case "GalilLipsel":
                     return new GalilLipselAxis(module, name);
+                case "GalilRotation":
+                    return new GalilRotationAxis(module, name);
                 default:
                     return null;
             }

CyberX8_RT/Devices/AXIS/GalilRotation/GalilRotationAxis.cs

@@ -0,0 +1,364 @@
+using Aitex.Core.RT.DataCenter;
+using Aitex.Core.RT.Event;
+using Aitex.Core.RT.Fsm;
+using Aitex.Core.RT.Log;
+using Aitex.Core.RT.Routine;
+using Aitex.Core.RT.SCCore;
+using Aitex.Core.Util;
+using MECF.Framework.Common.CommonData.PUF;
+using MECF.Framework.Common.TwinCat;
+using CyberX8_Core;
+using CyberX8_RT.Modules;
+using System;
+using System.Collections.Generic;
+using System.Data;
+using System.Linq;
+using System.Reflection;
+using System.Text;
+using System.Threading.Tasks;
+using CyberX8_RT.Devices.AXIS.Galil;
+
+namespace CyberX8_RT.Devices.AXIS.GalilLipsel
+{
+    public class GalilRotationAxis : JetAxisBase
+    {
+        #region 内部变量
+        /// <summary>
+        /// Home routine
+        /// </summary>
+        private GalilRotationHomeRoutine _homeRoutine;
+        /// <summary>
+        /// 运动Routine
+        /// </summary>
+        private GalilRotationProfilePositionRoutine _profilePositionRoutine;
+        /// <summary>
+        /// Switch On Routine
+        /// </summary>
+        private GalilRotationSwitchOnRoutine _switchOnRoutine;
+        /// <summary>
+        /// Switch Off Routine
+        /// </summary>
+        private GalilRotationSwitchOffRoutine _switchOffRoutine;
+        /// <summary>
+        /// Stop Position
+        /// </summary>
+        private GalilRotationStopPositionRoutine _stopPositionRoutine;
+        #endregion
+        /// <summary>
+        /// 构造函数
+        /// </summary>
+        /// <param name="Module"></param>
+        public GalilRotationAxis(string Module, string name) : base(Module, name)
+        {
+        }
+        /// <summary>
+        /// 初始化参数
+        /// </summary>
+        protected override void InitializeParameter()
+        {
+            _accelerationRatio = 1;
+            _speedRatio = 1;
+        }
+        /// <summary>
+        /// 初始化Routine
+        /// </summary>
+        protected override void InitializeRoutine()
+        {
+            _homeRoutine = new GalilRotationHomeRoutine($"{Module}.{Name}", this);
+            _profilePositionRoutine = new GalilRotationProfilePositionRoutine($"{Module}.{Name}", this);
+            _switchOnRoutine = new GalilRotationSwitchOnRoutine($"{Module}.{Name}", this);
+            _switchOffRoutine = new GalilRotationSwitchOffRoutine($"{Module}.{Name}", this);
+            _stopPositionRoutine = new GalilRotationStopPositionRoutine($"{Module}.{Name}", this);
+        }
+        /// <summary>
+        /// 中止操作
+        /// </summary>
+        /// <param name="cmd"></param>
+        /// <param name="args"></param>
+        /// <returns></returns>
+        public override bool StopPositionOperation()
+        {
+            if (!IsRun)
+            {
+                return true;
+            }
+            if (_profilePositionRoutine.Monitor() == RState.Running)
+            {
+                _profilePositionRoutine.Abort();
+            }
+            _status = _stopPositionRoutine.Start();
+            _currentOperation = MotionOperation.StopPosition;
+            return true;
+        }
+
+        /// <summary>
+        /// 更新StatusWord
+        /// </summary>
+        /// <param name="status"></param>
+        public override void UpdateStatusWord(ushort status)
+        {
+        }
+        /// <summary>
+        /// EnableOperation
+        /// </summary>
+        public override bool EnableOperation()
+        {
+            return WriteControlWord(0x0F);
+        }
+        /// <summary>
+        /// Home
+        /// </summary>
+        public override bool Home()
+        {
+            bool result = base.Home();
+            if (!result)
+            {
+                return false;
+            }
+            MotionData.IsHomed = false;
+            _homeRoutine.Start(_homeTimeout, _galilAxisConfig.HomingAcceleration,_galilAxisConfig.HomingDeceleration,_galilAxisConfig.HomingSpeed,_galilAxisConfig.HomingOffset);
+            IsHomeSwitchedTriggered = false;
+            return true;
+        }
+
+
+        /// <summary>
+        /// 停止 
+        /// </summary>
+        public override void Stop()
+        {
+        }
+        /// <summary>
+        /// SwitchOff
+        /// </summary>
+        public override bool SwitchOff()
+        {
+            if (_status == RState.Running)
+            {
+                EV.PostAlarmLog($"{Module}.{Name}", eEvent.ERR_AXIS, $"{Module}.{Name} current execute {_currentOperation},cannot switchoff");
+                return false;
+            }
+            _currentOperation = MotionOperation.SwitchOff;
+            _switchOffRoutine.Start();
+            _status = RState.Running;
+            return true;
+        }
+        /// <summary>
+        /// SwitchOn
+        /// </summary>
+        public override bool SwitchOn()
+        {
+            if (_status == RState.Running)
+            {
+                EV.PostAlarmLog($"{Module}.{Name}", eEvent.ERR_AXIS, $"{Module}.{Name} current execute {_currentOperation},cannot SwitchOn");
+                return false;
+            }
+            _currentOperation = MotionOperation.SwitchOn;
+            _switchOnRoutine.Start();
+            _status = RState.Running;
+            return true;
+        }
+        /// <summary>
+        /// 定时器
+        /// </summary>
+        /// <returns></returns>
+        public override bool OnTimer()
+        {
+            if (_status == RState.Running)
+            {
+                if (_currentOperation == MotionOperation.Position)
+                {
+                    RState state = _profilePositionRoutine.Monitor();
+                    if (state == RState.End)
+                    {
+                        _inTargetPosition = false;
+                        EndOperation();
+                        _status = RState.End;
+                        LOG.WriteLog(eEvent.INFO_AXIS, $"{Module}.{Name}", $"Position Complete,Current Position {MotionData.MotorPosition}");
+                    }
+                    else if (state == RState.Failed || state == RState.Timeout)
+                    {
+                        _inTargetPosition = false;
+                        EndOperation();
+                        _status = RState.Failed;
+                        LOG.WriteLog(eEvent.ERR_AXIS, $"{Module}.{Name}", $"Profile Position error {_profilePositionRoutine.ErrorMsg}");
+                    }
+                }
+
+                else if (_currentOperation == MotionOperation.Home)
+                {
+                    RState state = _homeRoutine.Monitor();
+                    if (state == RState.End)
+                    {
+                        MotionData.IsHomed = true;
+                        IsHomed = true;
+                        EndOperation();
+                        _status = RState.End;
+                    }
+                    else if (state == RState.Failed || state == RState.Timeout)
+                    {
+                        EndOperation();
+                        _status = RState.Failed;
+                        LOG.WriteLog(eEvent.ERR_AXIS, $"{Module}.{Name}", "Home error");
+                    }
+                }
+                else if (_currentOperation == MotionOperation.SwitchOn)
+                {
+                    RState state = _switchOnRoutine.Monitor();
+                    if (state == RState.End)
+                    {
+                        EndOperation();
+                        _status = RState.End;
+                    }
+                    else if (state == RState.Failed || state == RState.Timeout)
+                    {
+                        EndOperation();
+                        _status = RState.Failed;
+                        LOG.WriteLog(eEvent.ERR_AXIS, $"{Module}.{Name}", "Switch On error");
+                    }
+                }
+                else if (_currentOperation == MotionOperation.SwitchOff)
+                {
+                    RState state = _switchOffRoutine.Monitor();
+                    if (state == RState.End)
+                    {
+                        EndOperation();
+                        _status = RState.End;
+                    }
+                    else if (state == RState.Failed || state == RState.Timeout)
+                    {
+                        EndOperation();
+                        _status = RState.Failed;
+                        LOG.WriteLog(eEvent.ERR_AXIS, $"{Module}.{Name}", "Switch Off error");
+                    }
+                }
+                else if (_currentOperation == MotionOperation.StopPosition)
+                {
+                    RState state = _stopPositionRoutine.Monitor();
+                    if (state == RState.End)
+                    {
+                        EndOperation();
+                        _status = RState.End;
+                    }
+                    else if (state == RState.Failed || state == RState.Timeout)
+                    {
+                        EndOperation();
+                        _status = RState.Failed;
+                        LOG.WriteLog(eEvent.ERR_AXIS, $"{Module}.{Name}", "StopPosition error");
+                    }
+                }
+            }
+            JudgeRunMonitor();
+            return true;
+        }
+
+        /// <summary>
+        /// 位置
+        /// </summary>
+        /// <param name="targetPoint"></param>
+        public override bool ProfilePosition(int targetPoint, int profileVelocity, int profileAcceleration, int profileDeceleration, bool judgeTorqueLimit = true)
+        {
+            if (_status == RState.Running)
+            {
+                LOG.WriteLog(eEvent.WARN_AXIS, $"{Module}.{Name}", $"current execute {_currentOperation},cannot profile position");
+                return false;
+            }
+            if (profileAcceleration == 0)
+            {
+                profileAcceleration = _profileAcceleration;
+            }
+            if (profileDeceleration == 0)
+            {
+                profileDeceleration = _profileDeceleration;
+            }
+            _status = _profilePositionRoutine.Start(targetPoint, profileVelocity, profileAcceleration, profileDeceleration, judgeTorqueLimit);
+            _currentOperation = MotionOperation.Position;
+            _inTargetPosition = false;
+            return true;
+        }
+        /// <summary>
+        /// 改变速度
+        /// </summary>
+        /// <param name="speed"></param>
+        /// <returns></returns>
+        public override bool ChangeSpeed(int speed)
+        {
+            bool result = WriteControlWord(0x2F);
+            if (!result)
+            {
+                return false;
+            }
+            result = BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.ProfileVelocity", speed);
+            if (!result)
+            {
+                return false;
+            }
+            result = WriteControlWord(0x3F);
+            return result;
+        }
+
+        /// <summary>
+        /// 改变速度加速度
+        /// </summary>
+        /// <param name="speed"></param>
+        /// <returns></returns>
+        public override bool ChangeSpeedAcceleration(int speed, int acceleration, int deceleration)
+        {
+            bool result = WriteControlWord(0x2F);
+            if (!result)
+            {
+                return false;
+            }
+            result = BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.ProfileVelocity", speed);
+            BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{PROFILE_ACCEL}", acceleration);
+            BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{PROFILE_DECEL}", deceleration);
+            if (!result)
+            {
+                return false;
+            }
+            result = WriteControlWord(0x3F);
+            return result;
+        }
+        /// <summary>
+        /// KeyDown事件
+        /// </summary>
+        /// <param name="arg"></param>
+        /// <param name="value"></param>
+        protected override void AxisKeyDown(string arg, double value)
+        {
+            switch (arg)
+            {
+                case PROFILE_VELOCITY:
+                    _profileVelocity = CalculateMultiplySpeedRatio(CalculateValueMultiplyScale(value));
+                    _commandMotionData.FileProfileVelocity = value;
+                    BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{arg}", _profileVelocity);
+                    break;
+                case PROFILE_ACCEL:
+                    _profileAcceleration = CalculateDivideAccelerationRatio(CalculateValueMultiplyScale(value));
+                    _commandMotionData.FileAcceleration = value;
+                    BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{arg}", _profileAcceleration);
+                    break;
+                case PROFILE_DECEL:
+                    _profileDeceleration = CalculateDivideAccelerationRatio(CalculateValueMultiplyScale(value));
+                    _commandMotionData.FileDeceleration = value;
+                    BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{arg}", _profileDeceleration);
+                    break;
+                case HOMING_VELOCITY:
+                    _profileHomingVelocity = CalculateMultiplySpeedRatio(CalculateValueMultiplyScale(value));
+                    _commandMotionData.FileHomingVelocity = value;
+                    TwincatCoeManager.Instance.WriteVariableValue($"{Module}.{Name}", arg, _profileHomingVelocity);
+                    break;
+                case HOMING_VELOCITY_SLOW:
+                    _profileHomingVelocitySlow = CalculateMultiplySpeedRatio(CalculateValueMultiplyScale(value));
+                    _commandMotionData.FileHomingVelocitySlow = value;
+                    TwincatCoeManager.Instance.WriteVariableValue($"{Module}.{Name}", arg, _profileHomingVelocitySlow);
+                    break;
+                case HOMING_ACCEL:
+                    _profileHomingAccel = CalculateDivideAccelerationRatio(CalculateValueMultiplyScale(value));
+                    _commandMotionData.FileHomingAccel = value;
+                    TwincatCoeManager.Instance.WriteVariableValue($"{Module}.{Name}", arg, _profileHomingAccel);
+                    break;
+            }
+        }
+    }
+}

CyberX8_RT/Devices/AXIS/GalilRotation/GalilRotationHomeRoutine.cs

@@ -0,0 +1,97 @@
+using Aitex.Core.RT.Log;
+using Aitex.Core.RT.Routine;
+using MECF.Framework.Common.CommonData.PUF;
+using MECF.Framework.Common.Routine;
+using CyberX8_Core;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace CyberX8_RT.Devices.AXIS.Galil
+{
+   
+    public class GalilRotationHomeRoutine : RoutineBase, IRoutine
+    {
+        private enum HomeStep
+        {
+            HomingAcceleration,
+            HomingDeceleration,
+            HomingSpeed,
+            WriteCNType,
+            SetHomeModel,
+            StartMotion,
+            Delay,
+            CheckHomingEnd,
+            DPDelay,
+            DP,
+            End
+        }
+
+        #region 常量
+        private const int HOME_STOP_CODE = 10;
+        #endregion
+
+        #region 内部变量
+        private JetAxisBase _axis;
+        private int _timeout = 5000;
+        private int _homingAcceleration = 0;
+        private int _homingDeceleration = 0;
+        private int _homingSpeed = 0;
+        private int _homingOffset = 0;
+        private int _cnType = 0;
+        #endregion
+        public GalilRotationHomeRoutine(string module,JetAxisBase axis) : base(module)
+        {
+            _axis = axis;
+        }
+
+        public void Abort()
+        {
+            Runner.Stop("Manual Abort");
+        }
+
+        public RState Monitor()
+        {
+            Runner.Run(HomeStep.HomingAcceleration, () => { return _axis.WriteAcceleration(_homingAcceleration); }, _delay_1ms)
+                .Run(HomeStep.HomingDeceleration, () => { return _axis.WriteDeceleration(_homingDeceleration); }, _delay_1ms)
+                .Run(HomeStep.HomingSpeed, () => { return _axis.WriteSpeed(_homingSpeed); }, _delay_1ms)
+                //增加CN类型用于区分process transporter gantry与elevator home方向相反
+                .RunIf(HomeStep.WriteCNType, _cnType!=0, () => { return _axis.WriteCNCommand($",{_cnType}"); },_delay_1ms)
+                .Run(HomeStep.SetHomeModel, () => { return _axis.WriteFIAxisCommand(); }, _delay_1ms)
+                .Run(HomeStep.StartMotion, () => { return _axis.WriteStartMotion(); }, _delay_1ms)
+                .Delay(HomeStep.Delay,500)
+                .WaitWithStopCondition(HomeStep.CheckHomingEnd, () => { 
+                    return _axis.MotionData.StopCode == HOME_STOP_CODE&&!_axis.IsRun; },CheckErrorOrWarning,_timeout)
+                .Delay(HomeStep.DPDelay,1000)
+                .Run(HomeStep.DP, () => { return _axis.WriteDP(_homingOffset); }, _delay_1ms)
+                .End(HomeStep.End,NullFun,100);
+            return Runner.Status;
+        }
+        /// <summary>
+        /// 检验是否出错或告警
+        /// </summary>
+        /// <returns></returns>
+        private bool CheckErrorOrWarning()
+        {
+            //byte stopCode = _axis.MotionData.StopCode;
+            //if (stopCode != 0 && stopCode != HOME_STOP_CODE)
+            //{
+            //    LOG.WriteLog(eEvent.ERR_AXIS, Module, $"axis home stopcode is {stopCode}");
+            //    return true;
+            //}
+            return false;
+        }
+        public RState Start(params object[] objs)
+        {
+            _timeout = (int)objs[0];
+            _homingAcceleration = (int)objs[1];
+            _homingDeceleration = (int)objs[2];
+            _homingSpeed = (int)objs[3];
+            _homingOffset = (int)objs[4];
+            _cnType = objs.Length >= 6 ? (int)objs[5] : 0;
+            return Runner.Start(Module, "Home");
+        }
+    }
+}

CyberX8_RT/Devices/AXIS/GalilRotation/GalilRotationProfilePositionRoutine.cs

@@ -0,0 +1,135 @@
+using Aitex.Core.RT.Log;
+using Aitex.Core.RT.Routine;
+using MECF.Framework.Common.CommonData.PUF;
+using MECF.Framework.Common.Routine;
+using MECF.Framework.Common.TwinCat;
+using CyberX8_Core;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace CyberX8_RT.Devices.AXIS.GalilLipsel
+{
+   
+    public class GalilRotationProfilePositionRoutine : RoutineBase, IRoutine
+    {
+        private enum ProfilePositionStep
+        {
+            WriteAcceleration,
+            WriteDeceleration,
+            WriteSpeed,
+            WriteTargetPosition,
+            StartMotion,
+            Delay,
+            CheckRunEnd,
+            End
+        }
+        #region 常量 
+        private const string PROFILE_VELOCITY = "ProfileVelocity";
+        private const string PROFILE_ACCEL = "ProfileAccel";
+        private const string PROFILE_DECEL = "ProfileDecel";
+        private const string TARGET_POSITION = "TargetPosition";
+        private const int MOTION_STOP_CODE = 1;
+        #endregion
+        #region 内部变量
+        private JetAxisBase _axis;
+        private int _profileVelocity = 0;
+        private int _profileAcceleration = 0;
+        private int _profileDeceleration = 0;
+        private int _targetPosition;
+        private bool _judgeTorqueLimit = false;
+        #endregion
+        public GalilRotationProfilePositionRoutine(string module,JetAxisBase axis) : base(module)
+        {
+            _axis = axis;
+        }
+
+        public void Abort()
+        {
+            Runner.Stop("Manual Abort");
+        }
+
+        public RState Monitor()
+        {
+            Runner.Run(ProfilePositionStep.WriteAcceleration, () => { return _axis.WriteAcceleration(_profileAcceleration); },_delay_1ms)
+                .Run(ProfilePositionStep.WriteDeceleration, () => { return _axis.WriteDeceleration(_profileDeceleration); }, _delay_1ms)
+                .Run(ProfilePositionStep.WriteSpeed, () => { return _axis.WriteSpeed(_profileVelocity); }, _delay_1ms)
+                .Run(ProfilePositionStep.WriteTargetPosition, () => { return _axis.WriteAbsolutePosition(_targetPosition); }, _delay_1ms)
+                .Run(ProfilePositionStep.StartMotion, () =>_axis.WriteStartMotion(), _delay_1ms)
+                .Delay(ProfilePositionStep.Delay,500)
+                .WaitWithStopCondition(ProfilePositionStep.CheckRunEnd,CheckRun,CheckRunStop)
+                .End(ProfilePositionStep.End,NullFun,100);
+            return Runner.Status;
+        }
+        /// <summary>
+        /// 检验是否正常运行
+        /// </summary>
+        /// <returns></returns>
+        private bool CheckRun()
+        {
+            bool isInTargetPosition = _axis.JudgeCurrentPositionIsInTargetPosition(_targetPosition);
+            if (_judgeTorqueLimit)
+            {
+                if (!_axis.IsRun && isInTargetPosition)
+                {
+                    return true;
+                }
+                return false;
+            }
+            else
+            {
+                bool isTorqueOver = TorqueIsOverLimit();
+                //过载同时掉电
+                if (isTorqueOver && !_axis.IsSwitchOn)
+                {
+                    return true;
+                }
+                if (!_axis.IsRun && isInTargetPosition)
+                {
+                    return true;
+                }
+                return false;
+            }
+        }
+        /// <summary>
+        /// Torque是否过载
+        /// </summary>
+        /// <returns></returns>
+        private bool TorqueIsOverLimit()
+        {
+            return !_axis.MotionData.TorqueLimited;
+        }
+        /// <summary>
+        /// 检验是否停止 
+        /// </summary>
+        /// <returns></returns>
+        private bool CheckRunStop()
+        {
+            //没有运动
+            bool isStop = !_axis.IsRun&&!_axis.JudgeCurrentPositionIsInTargetPosition(_targetPosition);
+            if(_judgeTorqueLimit)
+            {
+                if (isStop)
+                {
+                    ErrorMsg = "position is stop";
+                    return true;
+                }
+            }
+            return false;
+        }
+        public RState Start(params object[] objs)
+        {
+            _targetPosition = (int)objs[0];
+            _profileVelocity = (int)objs[1];
+            _profileAcceleration = (int)objs[2];
+            _profileDeceleration = (int)objs[3];
+            if(objs.Length>=5)
+            {
+                _judgeTorqueLimit = (bool)objs[4];
+            }
+            return Runner.Start(Module, "ProfilePosition");
+        }
+    }
+}

CyberX8_RT/Devices/AXIS/GalilRotation/GalilRotationStopPositionRoutine.cs

@@ -0,0 +1,91 @@
+using Aitex.Core.RT.Log;
+using Aitex.Core.RT.Routine;
+using CyberX8_Core;
+using MECF.Framework.Common.Beckhoff.AxisProvider;
+using MECF.Framework.Common.CommonData.PUF;
+using MECF.Framework.Common.Routine;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace CyberX8_RT.Devices.AXIS.GalilLipsel
+{
+    public class GalilRotationStopPositionRoutine : RoutineBase, IRoutine
+    {
+        private enum StopPositionStep
+        {
+            SetStop,
+            WriteTargePosition,
+            End
+        }
+        #region 常量
+        private const string TARGET_POSITION = "TargetPosition";
+        private const int STOP_CODE = 4;
+        #endregion
+
+        #region 内部变量
+        private JetAxisBase _axis;
+        private int _targetPosition;
+        /// <summary>
+        /// SRD rotation Provider对象
+        /// </summary>
+        private BeckhoffProviderAxis _rotationProviderAxis;
+        #endregion
+
+        public GalilRotationStopPositionRoutine(string module, JetAxisBase axis) : base(module)
+        {
+            _axis = axis;
+        }
+
+        public void Abort()
+        {
+            Runner.Stop("Manual Abort");
+        }
+
+        public RState Monitor()
+        {
+            Runner.Run(StopPositionStep.SetStop, SetStop, CheckStop, _delay_2s)
+                .Run(StopPositionStep.WriteTargePosition, () => { return WriteTargetPosition(); }, NullFun, 100)
+                .End(StopPositionStep.End, NullFun, 100);
+            return Runner.Status;
+        }
+        /// <summary>
+        /// 设置停止
+        /// </summary>
+        /// <returns></returns>
+        private bool SetStop()
+        {
+            return _axis.WriteStop();
+        }
+        /// <summary>
+        /// 检验是否停止
+        /// </summary>
+        /// <returns></returns>
+        private bool CheckStop()
+        {
+            return _axis.MotionData.StopCode ==STOP_CODE&&!_axis.IsRun;
+        }
+        /// <summary>
+        /// 设置目标位置为当前位置
+        /// </summary>
+        /// <param name="targetPosition"></param>
+        private bool WriteTargetPosition()
+        {
+            double scale = _rotationProviderAxis.ScaleFactor;
+            _targetPosition = (int)Math.Ceiling(_axis.MotionData.MotorPosition * scale);
+            return _axis.WriteAbsolutePosition(_targetPosition);
+        }
+        public RState Start(params object[] objs)
+        {
+            _rotationProviderAxis = BeckhoffAxisProviderManager.Instance.GetAxisProvider($"{Module}");
+            if (_rotationProviderAxis == null)
+            {
+                LOG.WriteLog(eEvent.ERR_AXIS, Module, $"{Module} Axist Provider is not exist");
+                return RState.Failed;
+            }
+            return Runner.Start(Module, "Stop Position");
+        }
+    }
+}

CyberX8_RT/Devices/AXIS/GalilRotation/GalilRotationSwitchOffRoutine.cs

@@ -0,0 +1,58 @@
+using Aitex.Core.RT.Routine;
+using MECF.Framework.Common.CommonData.PUF;
+using MECF.Framework.Common.Routine;
+using CyberX8_Core;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace CyberX8_RT.Devices.AXIS.GalilLipsel
+{
+   
+    public class GalilRotationSwitchOffRoutine : RoutineBase, IRoutine
+    {
+        private enum SwitchOffStep
+        {
+            SwitchOff,
+            End
+        }
+        #region 内部变量
+        private JetAxisBase _axis;
+        #endregion
+        public GalilRotationSwitchOffRoutine(string module,JetAxisBase axis) : base(module)
+        {
+            _axis = axis;
+        }
+
+        public void Abort()
+        {
+            Runner.Stop("Manual Abort");
+        }
+
+        public RState Monitor()
+        {
+            Runner.Run(SwitchOffStep.SwitchOff,SwitchOff,CheckSwitchOff,1000)
+                .End(SwitchOffStep.End,NullFun,100);
+            return Runner.Status;
+        }
+        /// <summary>
+        /// 下电
+        /// </summary>
+        /// <returns></returns>
+        public bool SwitchOff()
+        {
+            return _axis.WriteSwitchOff();
+        }
+
+        private bool CheckSwitchOff()
+        {
+            return !_axis.IsSwitchOn;
+        }
+        public RState Start(params object[] objs)
+        {
+            return Runner.Start(Module, "Switch Off");
+        }
+    }
+}

CyberX8_RT/Devices/AXIS/GalilRotation/GalilRotationSwitchOnRoutine.cs

@@ -0,0 +1,77 @@
+using Aitex.Core.RT.Routine;
+using MECF.Framework.Common.CommonData.PUF;
+using MECF.Framework.Common.Routine;
+using CyberX8_Core;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace CyberX8_RT.Devices.AXIS.GalilLipsel
+{
+   
+    public class GalilRotationSwitchOnRoutine : RoutineBase, IRoutine
+    {
+        private enum SwitchOnStep
+        {
+            WriteTargetPosition,
+            SwitchOn,
+            End
+        }
+
+        #region 常量 
+        private const string TARGET_POSITION = "TargetPosition";
+        #endregion
+
+        #region 内部变量
+        private JetAxisBase _axis;
+        #endregion
+        public GalilRotationSwitchOnRoutine(string module,JetAxisBase axis) : base(module)
+        {
+            _axis = axis;
+        }
+
+        public void Abort()
+        {
+            Runner.Stop("Manual Abort");
+        }
+
+        public RState Monitor()
+        {
+            Runner.Run(SwitchOnStep.WriteTargetPosition, WriteTargetPosition, _delay_1ms)
+                .Run(SwitchOnStep.SwitchOn, SwitchOn,CheckSwitchOn,1000)
+                .End(SwitchOnStep.End, NullFun, _delay_1ms);
+            return Runner.Status;
+        }
+        /// <summary>
+        /// 上电
+        /// </summary>
+        /// <returns></returns>
+        private bool SwitchOn()
+        {
+            return _axis.WriteSwitchOn();
+        }
+        /// <summary>
+        /// 当前位置写入Target
+        /// </summary>
+        /// <returns></returns>
+        private bool WriteTargetPosition()
+        {
+            int targetPosition =_axis.CalculateValueMultiplyScale(_axis.MotionData.MotorPosition);
+            return _axis.WriteAbsolutePosition(targetPosition);
+        }
+        /// <summary>
+        /// 检验上电状态
+        /// </summary>
+        /// <returns></returns>
+        private bool CheckSwitchOn()
+        {
+            return _axis.IsSwitchOn;
+        }
+        public RState Start(params object[] objs)
+        {
+            return Runner.Start(Module, "Switch On");
+        }
+    }
+}

CyberX8_RT/Devices/AXIS/JetAxisBase.cs

@@ -1184,7 +1184,14 @@ namespace CyberX8_RT.Devices.AXIS
         {
             return GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "HM", null);
         }
-
+        /// <summary>
+        /// Home 电机
+        /// </summary>
+        /// <returns></returns>
+        public bool WriteFIAxisCommand()
+        {
+            return GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "FI", null);
+        }
         /// <summary>
         /// Home 电机
         /// </summary>