Add FI Home operation in GalilSimulator; Revise SRD related Routines;

CyberX8_RT/Config/Devices/GalilControllerCfg-Simulator.xml

@@ -251,15 +251,15 @@
 			<DI Name="DI8"  Address="8" Invert="false"/>
 			<DI Name="DI9"  Address="9" Invert="false"/>
 		</Dig_In>
-		<GalilAxisConfig Name="Rotation" Index="0" Type="Galil" CNType="-1">
-			<Speed>200000</Speed>
-			<Acceleration>200000</Acceleration>
-			<Deceleration>200000</Deceleration>
-			<HomingSpeed>10000</HomingSpeed>
+		<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>200000</HomingAcceleration>
-			<HomingDeceleration>400000</HomingDeceleration>
+			<HomingAcceleration>84000</HomingAcceleration>
+			<HomingDeceleration>420000</HomingDeceleration>
 			<ReverseSoftwareLimit>0</ReverseSoftwareLimit>
 			<ForwardSoftwareLimit>0</ForwardSoftwareLimit>
 			<NegativeTorqueLimit>130</NegativeTorqueLimit>

CyberX8_RT/Config/Devices/GalilControllerCfg.xml

@@ -196,19 +196,7 @@
 			<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>
+	<GalilDeviceConfig Module="SRD1" IpAddress="10.0.0.24" Port="58678" SendTimeout="2000" RecvTimeout="2000" GalilType="Galil21">		
 		<GalilAxisConfig Name="Rotation" Index="0" Type="GalilRotation" CNType="-1">
 			<Speed>529400</Speed>
 			<Acceleration>84000</Acceleration>

CyberX8_RT/Devices/AXIS/GalilRotation/GalilRotationAxis.cs

@@ -17,6 +17,8 @@ using System.Reflection;
 using System.Text;
 using System.Threading.Tasks;
 using CyberX8_RT.Devices.AXIS.Galil;
+using MECF.Framework.Common.Device.Galil;
+using MECF.Framework.Common.Utilities;
 
 namespace CyberX8_RT.Devices.AXIS.GalilLipsel
 {
@@ -50,6 +52,9 @@ namespace CyberX8_RT.Devices.AXIS.GalilLipsel
         /// <param name="Module"></param>
         public GalilRotationAxis(string Module, string name) : base(Module, name)
         {
+            //deg->RPM
+            _commandMotionData.FileProfileVelocity /= 6;
+            _commandMotionData.FileHomingVelocity /= 6;
         }
         /// <summary>
         /// 初始化参数
@@ -57,7 +62,7 @@ namespace CyberX8_RT.Devices.AXIS.GalilLipsel
         protected override void InitializeParameter()
         {
             _accelerationRatio = 1;
-            _speedRatio = 1;
+            _speedRatio = 6;
         }
         /// <summary>
         /// 初始化Routine
@@ -331,22 +336,22 @@ namespace CyberX8_RT.Devices.AXIS.GalilLipsel
                 case PROFILE_VELOCITY:
                     _profileVelocity = CalculateMultiplySpeedRatio(CalculateValueMultiplyScale(value));
                     _commandMotionData.FileProfileVelocity = value;
-                    BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{arg}", _profileVelocity);
+                    GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "SP", BeckhoffVelocityUtil.ConvertVelocityToDegPerSecondByRPM(_profileVelocity));
                     break;
                 case PROFILE_ACCEL:
                     _profileAcceleration = CalculateDivideAccelerationRatio(CalculateValueMultiplyScale(value));
-                    _commandMotionData.FileAcceleration = value;
-                    BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{arg}", _profileAcceleration);
+                    _commandMotionData.FileAcceleration = value; 
+                    GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "AC", _profileAcceleration);
                     break;
                 case PROFILE_DECEL:
                     _profileDeceleration = CalculateDivideAccelerationRatio(CalculateValueMultiplyScale(value));
                     _commandMotionData.FileDeceleration = value;
-                    BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{arg}", _profileDeceleration);
+                    GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "DC", _profileDeceleration);
                     break;
                 case HOMING_VELOCITY:
                     _profileHomingVelocity = CalculateMultiplySpeedRatio(CalculateValueMultiplyScale(value));
                     _commandMotionData.FileHomingVelocity = value;
-                    TwincatCoeManager.Instance.WriteVariableValue($"{Module}.{Name}", arg, _profileHomingVelocity);
+                    GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "SP", BeckhoffVelocityUtil.ConvertVelocityToDegPerSecondByRPM(_profileHomingVelocity));
                     break;
                 case HOMING_VELOCITY_SLOW:
                     _profileHomingVelocitySlow = CalculateMultiplySpeedRatio(CalculateValueMultiplyScale(value));
@@ -356,7 +361,7 @@ namespace CyberX8_RT.Devices.AXIS.GalilLipsel
                 case HOMING_ACCEL:
                     _profileHomingAccel = CalculateDivideAccelerationRatio(CalculateValueMultiplyScale(value));
                     _commandMotionData.FileHomingAccel = value;
-                    TwincatCoeManager.Instance.WriteVariableValue($"{Module}.{Name}", arg, _profileHomingAccel);
+                    GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "AC", _profileHomingAccel);
                     break;
             }
         }

CyberX8_RT/Devices/AXIS/JetAxisBase.cs

@@ -39,7 +39,7 @@ namespace CyberX8_RT.Devices.AXIS
         protected const string PROFILE_DECEL = "ProfileDecel";
         private const string TARGET_POSITION = "TargetPosition";
         private const string MOTOR_POSITION = "MotorPosition";
-        private const string ACTUAL_VELOCITY = "ActualVelocity";
+        private const string ACTUAL_VELOCITY = "Velocity";
         private const string ACTUAL_TORQUE = "ActualTorque";
         private const string POSITION_ERROR = "PositionError";
         private const string MOTION_DATA = "MotionData";
@@ -880,6 +880,10 @@ namespace CyberX8_RT.Devices.AXIS
             {
                 MotionData.FwdLimited = (bool)value;
             }
+            if(variable == ACTUAL_VELOCITY)
+            {
+                MotionData.ActualVelocity = (int)value;
+            }
         }
 
         /// <summary>

CyberX8_RT/Modules/SRD/SRDEntity.cs

@@ -881,9 +881,7 @@ namespace CyberX8_RT.Modules.SRD
         private bool InitializeHome(object[] param)
         {
             _isHomed = false;
-
-            //return _initializeHomeRoutine.Start() == RState.Running;
-            return true;
+            return _initializeHomeRoutine.Start() == RState.Running;
         }
         /// <summary>
         /// InitializeHome超时
@@ -892,20 +890,19 @@ namespace CyberX8_RT.Modules.SRD
         /// <returns></returns>
         private bool InitializeHomeTimeout(object[] param)
         {
-            //RState ret = _initializeHomeRoutine.Monitor();
-            //if (ret == RState.Failed || ret == RState.Timeout)
-            //{
-            //    PostMsg(SRDMSG.Error);
-            //    return false;
-            //}
+            RState ret = _initializeHomeRoutine.Monitor();
+            if (ret == RState.Failed || ret == RState.Timeout)
+            {
+                PostMsg(SRDMSG.Error);
+                return false;
+            }
 
-            //bool result = ret == RState.End;
-            //if (result)
-            //{
-            //    _isHomed = true;
-            //}
-            //return result;
-            return true;
+            bool result = ret == RState.End;
+            if (result)
+            {
+                _isHomed = true;
+            }
+            return result;
         }
         #endregion        
 

CyberX8_RT/Modules/SRD/SRDLoaderRoutine.cs

@@ -113,11 +113,11 @@ namespace CyberX8_RT.Modules.SRD
         private bool CheckPreCondition()
         {
             //Check Rotation Home
-            //if (!_rotationAxis.IsHomed)
-            //{
-            //    NotifyError(eEvent.ERR_SRD, "Rotation is not homed", 0);
-            //    return false;
-            //}
+            if (!_rotationAxis.IsHomed)
+            {
+                NotifyError(eEvent.ERR_SRD, "Rotation is not homed", 0);
+                return false;
+            }
             //Check Vacuum off
             if (!_srdCommon.CommonData.ChuckVacuum)
             {

CyberX8_RT/Modules/SRD/SRDRotationRoutine.cs

@@ -31,7 +31,7 @@ namespace CyberX8_RT.Modules.SRD
         /// <summary>
         /// ROTATION电机转速比例
         /// </summary>
-        private const int SPEED_RATIO = 10;
+        private const int SPEED_RATIO = 1;
         #endregion
 
         #region 内部变量
@@ -100,8 +100,7 @@ namespace CyberX8_RT.Modules.SRD
         /// </summary>
         /// <returns></returns>
         public bool StartRotation()
-        {
-            
+        {           
 
             if (!_rotationAxis.IsSwitchOn)
             {
@@ -123,6 +122,16 @@ namespace CyberX8_RT.Modules.SRD
             {
                 return false;
             }
+            if (_srdCommon.CommonData.LiftUp)
+            {
+                LOG.WriteLog(eEvent.ERR_SRD, _module, $"{_module} LiftUp is on");
+                return false;
+            }
+            if (_srdCommon.CommonData.LiftUpStatus)
+            {
+                LOG.WriteLog(eEvent.ERR_SRD, _module, $"{_module} LiftUp sensor is on");
+                return false;
+            }
             double _scale = _rotationProviderAxis.ScaleFactor;
             //获取当前Position
             double currentPosition = _rotationAxis.MotionData.MotorPosition;

CyberX8_RT/Modules/SRD/SRDRunRecipeRoutine.cs

@@ -42,7 +42,7 @@ namespace CyberX8_RT.Modules.SRD
         /// <summary>
         /// ROTATION电机转速比例
         /// </summary>
-        private const int SPEED_RATIO = 10;
+        private const int SPEED_RATIO = 1;
         #endregion
 
         #region 内部变量

CyberX8_RT/Modules/SRD/SRDUnloaderRoutine.cs

@@ -135,11 +135,11 @@ namespace CyberX8_RT.Modules.SRD
         private bool CheckPreCondition()
         {
             //Check Rotation Home
-            //if (!_rotationAxis.IsHomed)
-            //{
-            //    NotifyError(eEvent.ERR_SRD, "Rotation is not homed", 0);
-            //    return false;
-            //}
+            if (!_rotationAxis.IsHomed)
+            {
+                NotifyError(eEvent.ERR_SRD, "Rotation is not homed", 0);
+                return false;
+            }
             //Check LiftUp
             if (_srdCommon.CommonData.LiftUp)
             {

CyberX8_Simulator/Devices/GalilSocketSimulator.cs

@@ -37,6 +37,7 @@ namespace CyberX8_Simulator.Devices
         private const string ACTUAL_POSITION = "ActualPosition";
         private const string AUXILIARY_POSITION = "AuxiliaryPosition";
         private const string HOMING_SIGNAL = "HomingSignal";
+        private const string FI_HOMING_SIGNAL = "FIHomingSignal";
         private const string MOTION_SIGNAL = "MotionSignal";
         private const string STOP_SIGNAL = "StopSignal";      
        
@@ -552,6 +553,9 @@ namespace CyberX8_Simulator.Devices
                 case "HM"://回零
                     Home(axis);
                     break;
+                case "FI"://SRDRotation回零
+                    Home(axis);
+                    break;
                 case "DP"://设定motor position
                     SetMotorPosition(axis, value);
                     break;
@@ -596,7 +600,7 @@ namespace CyberX8_Simulator.Devices
         /// <param name="index"></param>
         private void UpdateAxisData(int index, SimulatorMotionData data)
         {
-            _galilControlData.GalilAxisDatas[index].Velocity = data.ActualVelocity;
+            _galilControlData.GalilAxisDatas[index].Velocity = data.TargetVelocity;
             _galilControlData.GalilAxisDatas[index].MotorPosition = (int)(data.ActualPosition / _motorPositionRateDic[_nameAxisList[index]]);
             _galilControlData.GalilAxisDatas[index].ReferencePosition = data.TargetPosition;
             _galilControlData.GalilAxisDatas[index].Torque = data.ActualTorque;
@@ -717,7 +721,14 @@ namespace CyberX8_Simulator.Devices
         {
             MotorSimulator.Instance.SetMotionData(_nameAxisList[axis - 'A'], HOMING_SIGNAL, true);
         }
-
+        /// <summary>
+        /// 电机FI回零
+        /// </summary>
+        /// <param name="axis"></param>
+        private void FI(char axis)
+        {
+            MotorSimulator.Instance.SetMotionData(_nameAxisList[axis - 'A'], FI_HOMING_SIGNAL, true);
+        }
         public void SetForwardLimit()
         {
 

CyberX8_Themes/UserControls/SRDMotionControl.xaml

@@ -149,7 +149,7 @@
                     <TextBlock Grid.Row="4" Text="rpm"  Grid.Column="3" Margin="2,0,2,0" TextAlignment="Center" VerticalAlignment="Center"  Background="Black" Foreground="Lime" FontSize="16" FontWeight="Bold">
                     </TextBlock>
 
-                    <Label Grid.Row="5" Grid.Column="0" Content="Home Accel/Decel" FontSize="16" FontWeight="Bold"/>
+                    <Label Grid.Row="5" Grid.Column="0" Content="Home Accel" FontSize="16" FontWeight="Bold"/>
                     <Control:NumbericTextBox Grid.Row="5" Grid.Column="1"  Margin="2,0,0,0" HorizontalAlignment="Left" VerticalAlignment="Center" Width="80"
                                          Height="22" Value="{Binding ElementName=self,Path=InputHomeAccelDecel,StringFormat={}{0:F2},Mode=TwoWay}" TextboxName="HomingAccel" 
                                          KeyOperation="{Binding ElementName=self,Path=KeyDownCommand}" />
@@ -222,11 +222,14 @@
                         <ColumnDefinition Width="139*"/>
                     </Grid.ColumnDefinitions>
                     <Label  Content="Rev Limit" FontSize="16" FontWeight="Bold" Margin="0,0,181,19" />
-                    <Ellipse Margin="34,24,249,20"  Width="16" Height="16"  Fill="{Binding MotionData.RevLimited, Converter={StaticResource boolToRedColor}, ElementName=self}"   Stroke="Silver"/>
+                    <!--<Ellipse Margin="34,24,249,20"  Width="16" Height="16"  Fill="{Binding MotionData.RevLimited, Converter={StaticResource boolToRedColor}, ElementName=self}"   Stroke="Silver"/>-->
+                    <Ellipse Margin="34,24,249,20"  Width="16" Height="16"  Fill="Gray"   Stroke="Silver"/>
                     <Label  Content="Home Switch" FontSize="16" FontWeight="Bold" Margin="160,0,7,19" />
-                    <Ellipse Margin="194,24,89,20"  Width="16" Height="16"  Fill="{Binding MotionData.HomedSwitched, Converter={StaticResource boolToColor}, ElementName=self}"   Stroke="Silver"/>
+                    <!--<Ellipse Margin="194,24,89,20"  Width="16" Height="16"  Fill="{Binding MotionData.HomedSwitched, Converter={StaticResource boolToColor}, ElementName=self}"   Stroke="Silver"/>-->
+                    <Ellipse Margin="194,24,89,20"  Width="16" Height="16"  Fill="Gray"   Stroke="Silver"/>
                     <Label  Content="Fwd Limit" FontSize="16" FontWeight="Bold" Margin="26,0,15,19" Grid.Column="1" />
-                    <Ellipse Margin="60,24,63,20"  Width="16" Height="16"  Fill="{Binding MotionData.FwdLimited, Converter={StaticResource boolToRedColor}, ElementName=self}"   Stroke="Silver" Grid.Column="1"/>
+                    <!--<Ellipse Margin="60,24,63,20"  Width="16" Height="16"  Fill="{Binding MotionData.FwdLimited, Converter={StaticResource boolToRedColor}, ElementName=self}"   Stroke="Silver" Grid.Column="1"/>-->
+                    <Ellipse Margin="60,24,63,20"  Width="16" Height="16"  Fill="Gray"   Stroke="Silver" Grid.Column="1"/>
 
                 </Grid>
             </Grid>

Framework/Common/Simulator/MotorSimulator.cs

@@ -31,6 +31,7 @@ namespace MECF.Framework.Common.Simulator
         private const string ACTUAL_POSITION = "ActualPosition";
         private const string AUXILIARY_POSITION = "AuxiliaryPosition";
         private const string HOMING_SIGNAL = "HomingSignal";
+        private const string FI_HOMING_SIGNAL = "FIHomingSignal";
         private const string MOTION_SIGNAL = "MotionSignal";
         private const string STOP_SIGNAL = "StopSignal";
         private const string FESTO_DATABUFFER_TRANSPORT1_LOCK = "ProcessTransporterLock";
@@ -282,6 +283,9 @@ namespace MECF.Framework.Common.Simulator
                 case HOMING_SIGNAL:
                     _motorNameDataDic[axisName].HomingSignal = true;
                     break;
+                case FI_HOMING_SIGNAL:
+                    _motorNameDataDic[axisName].FIHomingSignal = true;
+                    break;
                 case MOTION_SIGNAL:
                     _motorNameDataDic[axisName].MotionSignal = true;
                     break;
@@ -301,7 +305,7 @@ namespace MECF.Framework.Common.Simulator
             string name = motor.Key;
             //上电检查
             if (!motionData.SwitchSignal) return;
-            if (motionData.HomingSignal)
+            if (motionData.HomingSignal || motionData.FIHomingSignal)
             {
                 HomeOperation(motionData);
             }
@@ -322,7 +326,7 @@ namespace MECF.Framework.Common.Simulator
                 motionData.StopCode = 10;//HM操作停止码10
                 motionData.MotionSignal = false;
                 motionData.HomingSignal = false;
-                motionData.MotionPhase = MotionPhase.Accelerating;
+                //motionData.MotionPhase = MotionPhase.Accelerating;
                 //motionData.ActualPosition = 0;
                 motionData.ActualVelocity = 0;
             }            

Framework/Common/Simulator/SimulatorMotionData.cs

@@ -192,12 +192,19 @@ namespace MECF.Framework.Common.Simulator
             get { return _reverseLimit; }
             set { _reverseLimit = value; InvokePropertyChanged(nameof(ReverseLimit)); }
         }
+        [DataMember]
+        public bool FIHomingSignal
+        {
+            get { return _fIHomingSignal; }
+            set { _fIHomingSignal = value; InvokePropertyChanged(nameof(FIHomingSignal)); }
+        }
         #endregion
 
         #region 内部变量
 
         #region Signal
         private bool _switchSignal;
+        private bool _fIHomingSignal;
         private bool _homingSignal;
         private bool _motionSignal;       
         private bool _stopSignal;