commit 018891301f5fc8bffa3a8b92142ff48e981320f7 Author: berti_r Date: Tue Sep 9 09:47:45 2025 +0200 initial commit diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..e69de29 diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/workspace.xml b/.idea/workspace.xml new file mode 100644 index 0000000..a30dda7 --- /dev/null +++ b/.idea/workspace.xml @@ -0,0 +1,50 @@ + + + + + + { + "associatedIndex": 0 +} + + + + { + "keyToString": { + "ModuleVcsDetector.initialDetectionPerformed": "true", + "RunOnceActivity.ShowReadmeOnStart": "true", + "last_opened_file_path": "C:/Users/berti_r/Python_Projects/templates/motion_libs", + "nodejs_package_manager_path": "npm", + "vue.rearranger.settings.migration": "true" + } +} + + + + + + + + + + 1752073467700 + + + + + + \ No newline at end of file diff --git a/__pycache__/eAxisParameters.cpython-313.pyc b/__pycache__/eAxisParameters.cpython-313.pyc new file mode 100644 index 0000000..84604dc Binary files /dev/null and b/__pycache__/eAxisParameters.cpython-313.pyc differ diff --git a/__pycache__/motionFunctionsLib.cpython-313.pyc b/__pycache__/motionFunctionsLib.cpython-313.pyc new file mode 100644 index 0000000..db763bf Binary files /dev/null and b/__pycache__/motionFunctionsLib.cpython-313.pyc differ diff --git a/eAxisParameters.py b/eAxisParameters.py new file mode 100644 index 0000000..12ed20a --- /dev/null +++ b/eAxisParameters.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python + +from enum import * + +class E_AxisParameters(Enum): + # PLCopen specific parameters Index-Group 0x4000 + ID + CommandedPosition = 1 # lreal taken from NcToPlc + SWLimitFwd = 2 # lreal IndexOffset= 16#0001_000E + SWLimitBwd = 3 # lreal IndexOffset= 16#0001_000D + EnableLimitFwd = 4 # bool IndexOffset= 16#0001_000C + EnableLimitBwd = 5 # bool IndexOffset= 16#0001_000B + EnablePosLagMonitoring = 6 # bool IndexOffset= 16#0002_0010 + MaxPositionLag = 7 # lreal IndexOffset= 16#0002_0012 + AxisMaxVelocity = 8 # lreal IndexOffset= 16#0000_0027 + ActualVelocity = 10 # lreal taken from NcToPlc + CommandedVelocity = 11 # lreal taken from NcToPlc + AxisDefaultAcceleration = 12 # lreal IndexOffset= 16#0000_0101 + AxisDefaultDeceleration = 14 # lreal IndexOffset= 16#0000_0102 + AxisDefaultJerk = 16 # lreal IndexOffset= 16#0000_0103 + + # Beckhoff specific parameters Index-Group 0x4000 + ID + AxisId = 1000 # lreal IndexOffset= 16#0000_0001 + AxisManVelSlow = 1001 # lreal IndexOffset= 16#0000_0008 + AxisManVelFast = 1002 # lreal IndexOffset= 16#0000_0009 + AxisRapidTraverseVelocity = 1009 # lreal IndexOffset= 16#0000_000A + AxisVelocityToCam = 1012 # lreal IndexOffset= 16#0000_0006 + AxisVelocityFromCam = 1013 # lreal IndexOffset= 16#0000_0007 + AxisJogIncrementForward = 1014 # lreal IndexOffset= 16#0000_0018 + AxisJogIncrementBackward = 1015 # lreal IndexOffset= 16#0000_0019 + AxisMaxPosLagFilterTime = 1022 # lreal IndexOffset= 16#0002_0013 + AxisEnPositionRangeMonitoring = 1023 # bool IndexOffset= 16#0000_000F + AxisPositionRangeWindow = 1024 # lreal IndexOffset= 16#0000_0010 + AxisEnTargetPositionMonitoring = 1025 # bool IndexOffset= 16#0000_0015 + AxisTargetPositionWindow = 1026 # lreal IndexOffset= 16#0000_0016 + AxisTargetPositionMonitoringTime = 1027 # lreal IndexOffset= 16#0000_0017 + AxisEnInTargetTimeout = 1028 # bool IndexOffset= 16#0000_0029 + AxisInTargetTimeout = 1029 # lreal IndexOffset= 16#0000_002A + AxisEnMotionMonitoring = 1030 # bool IndexOffset= 16#0000_0011 + AxisMotionMonitoringWindow = 1031 # lreal IndexOffset= 16#0000_0028 + AxisMotionMonitoringTime = 1032 # lreal IndexOffset= 16#0000_0012 + AxisDelayTimeVeloPosition = 1033 # lreal IndexOffset= 16#0000_0104 + AxisEnLoopingDistance = 1034 # bool IndexOffset= 16#0000_0013 + AxisLoopingDistance = 1035 # lreal IndexOffset= 16#0000_0014 + AxisEnBacklashCompensation = 1036 # bool IndexOffset= 16#0000_002B + AxisBacklash = 1037 # lreal IndexOffset= 16#0000_002C + AxisEnDataPersistence = 1038 # bool IndexOffset= 16#0000_0030 + AxisRefVeloOnRefOutput = 1039 # lreal IndexOffset= 16#0003_0101 + AxisOverrideType = 1040 # lreal IndexOffset= 16#0000_0105 + # new since 4/2007 + AxisEncoderOffset = 1042 # lreal IndexOffset= 16#0001_0007 + AxisEncoderDirectionInverse = 1043 # bool IndexOffset= 16#0001_0008 + AxisEncoderMask = 1044 # dword IndexOffset= 16#0001_0015 + AxisEncoderModuloValue = 1045 # lreal IndexOffset= 16#0001_0009 + AxisModuloToleranceWindow = 1046 # lreal IndexOffset= 16#0001_001B + AxisEnablePosCorrection = 1047 # bool IndexOffset= 16#0001_0016 + AxisPosCorrectionFilterTime = 1048 # lreal IndexOffset= 16#0001_0017 + # new since 1/2010 + AxisUnitInterpretation = 1049 # lreal IndexOffset= 16#0000_0026 + AxisMotorDirectionInverse = 1050 # bool IndexOffset= 16#0003_0006 + # new since 1/2011 + AxisCycleTime = 1051 # lreal IndexOffset= 16#0000_0004 + # new since 5/2011 + AxisFastStopSignalType = 1052 # dword IndexOffset= 16#0000_001E + AxisFastAcc = 1053 # lreal IndexOffset= 16#0000_010A + AxisFastDec = 1054 # lreal IndexOffset= 16#0000_010B + AxisFastJerk = 1055 # lreal IndexOffset= 16#0000_010C + # new since 1/2012 *) + AxisEncoderScalingNumerator = 1056 # lreal IndexOffset= 16#0001_0023 - available in Tc3 + AxisEncoderScalingDenominator = 1057 # lreal IndexOffset= 16#0001_0024 - available in Tc3 + # new since 7/2016 + AxisMaximumAcceleration = 1058 # lreal IndexOffset= 16#0000_00F1 + AxisMaximumDeceleration = 1059 # lreal IndexOffset= 16#0000_00F2 + AxisVeloJumpFactor = 1060 # lreal IndexOffset= 16#0000_0106 + AxisToleranceBallAuxAxis = 1061 # lreal IndexOffset= 16#0000_0108 + AxisMaxPositionDeviationAuxAxis = 1062 # lreal IndexOffset= 16#0000_0109 + AxisErrorPropagationMode = 1063 # dword IndexOffset= 16#0000_001A + AxisErrorPropagationDelay = 1064 # lreal IndexOffset= 16#0000_001B + AxisCoupleSlaveToActualValues = 1065 # bool IndexOffset= 16#0000_001C + AxisAllowMotionCmdToSlaveAxis = 1066 # bool IndexOffset= 16#0000_0020 + AxisAllowMotionCmdToExtSetAxis = 1067 # bool IndexOffset= 16#0000_0021 + AxisEncoderSubMask = 1068 # dword IndexOffset= 16#0001_0108 + AxisEncoderReferenceSystem = 1069 # dword IndexOffset= 16#0001_0019 + AxisEncoderPositionFilterPT1 = 1070 # lreal IndexOffset= 16#0001_0010 + AxisEncoderVelocityFilterPT1 = 1071 # lreal IndexOffset= 16#0001_0011 + AxisEncoderAccelerationFilterPT1 = 1072 # lreal IndexOffset= 16#0001_0012 + AxisEncoderMode = 1073 # dword IndexOffset= 16#0001_000A + AxisEncoderHomingInvDirCamSearch = 1074 # bool IndexOffset= 16#0001_0101 + AxisEncoderHomingInvDirSyncSearch = 1075 # bool IndexOffset= 16#0001_0102 + AxisEncoderHomingCalibValue = 1076 # lreal IndexOffset= 16#0001_0103 + AxisEncoderReferenceMode = 1077 # dword IndexOffset= 16#0001_0107 + AxisRefVeloOutputRatio = 1078 # lreal IndexOffset= 16#0003_0102 + AxisDrivePositionOutputScaling = 1079 # lreal IndexOffset= 16#0003_0109 + AxisDriveVelocityOutputScaling = 1080 # lreal IndexOffset= 16#0003_0105 + AxisDriveVelocityOutputDelay = 1081 # lreal IndexOffset= 16#0003_010D + AxisDriveMinOutputLimitation = 1082 # lreal IndexOffset= 16#0003_000B + AxisDriveMaxOutputLimitation = 1083 # lreal IndexOffset= 16#0003_000C + AxisTorqueInputScaling = 1084 # lreal IndexOffset= 16#0003_0031 - available in Tc3 + AxisTorqueInputFilterPT1 = 1085 # lreal IndexOffset= 16#0003_0032 - available in Tc3 + AxisTorqueDerivationInputFilterPT1 = 1086 # lreal IndexOffset= 16#0003_0033 - available in Tc3 + AxisTorqueOutputScaling = 1087 # lreal IndexOffset= 16#0003_010B + AxisTorqueOutputDelay = 1088 # lreal IndexOffset= 16#0003_010F + AxisAccelerationOutputScaling = 1089 # lreal IndexOffset= 16#0003_010A + AxisAccelerationOutputDelay = 1090 # lreal IndexOffset= 16#0003_010E + AxisDrivePositionOutputSmoothFilterType = 1091 # dword IndexOffset= 16#0003_0110 + AxisDrivePositionOutputSmoothFilterTime = 1092 # lreal IndexOffset= 16#0003_0111 + AxisDrivePositionOutputSmoothFilterOrder = 1093 # dword IndexOffset= 16#0003_0112 + AxisDriveMode = 1094 # dword IndexOffset= 16#0003_000A + AxisDriftCompensationOffset = 1095 # lreal IndexOffset= 16#0003_0104 + AxisPositionControlKv = 1096 # lreal IndexOffset= 16#0002_0102 + AxisCtrlVelocityPreCtrlWeight = 1097 # lreal IndexOffset= 16#0002_000B + AxisControllerMode = 1098 # dword IndexOffset= 16#0002_000A + AxisCtrlAutoOffset = 1099 # bool IndexOffset= 16#0002_0110 + AxisCtrlAutoOffsetTimer = 1100 # lreal IndexOffset= 16#0002_0115 + AxisCtrlAutoOffsetLimit = 1101 # lreal IndexOffset= 16#0002_0114 + AxisSlaveCouplingControlKcp = 1102 # lreal IndexOffset= 16#0002_010F + AxisCtrlOutputLimit = 1103 # lreal IndexOffset= 16#0002_0100 + + # Beckhoff specific axis status information - READ ONLY Index-Group 0x4100 + ID + AxisTargetPosition = 2000 # lreal IndexOffset= 16#0000_0013 + AxisRemainingTimeToGo = 2001 # lreal IndexOffset= 16#0000_0014 + AxisRemainingDistanceToGo = 2002 # lreal IndexOffset= 16#0000_0022, 16#0000_0042 + + # Beckhoff specific axis functions + # read/write gear ratio of a slave + AxisGearRatio = 3000 # lreal read: IndexGroup=0x4100+ID IdxOffset=16#0000_0022 + # write:IndexGroup=0x4200+ID IdxOffset=16#0000_0042 + + # Beckhoff specific other parameters + # new since 1/2011 + NcSafCycleTime = 4000 # lreal IndexOffset= 16#0000_0010 + NcSvbCycleTime = 4001 # lreal IndexOffset= 16#0000_0012 diff --git a/motionFunctionsLib.py b/motionFunctionsLib.py new file mode 100644 index 0000000..c4c8837 --- /dev/null +++ b/motionFunctionsLib.py @@ -0,0 +1,1348 @@ +#!/usr/bin/env python + +""" +This file contains three classes: plc, axis and pneumatic + +It contains functions that interact with tc_mca_std_lib on a Beckhoff PLC. +""" +import sys, os +from datetime import datetime +import pyads as pyads +import time +from enum import * +from eAxisParameters import E_AxisParameters + + +class E_MotionFunctions(Enum): + eMoveAbsolute = 0 + eMoveRelative = 1 + eMoveVelocity = 2 + eMoveModulo = 3 + eGearInMultiMaster = 4 + eGearOut = 5 + eHome = 10 + eWriteParameter = 50 + eReadParameter = 60 + + +class E_HomingRoutines(Enum): + eNoHoming = 0 + eHomeToLimit_Bwd = 1 + eHomeToLimit_Fwd = 2 + #eHomeToLimit_Bwd_2Speeds = 3 + #eHomeToLimit_Fwd_2Speeds = 4 + + eHomeToRef_Bwd = 11 + eHomeToRef_Fwd = 12 + #eHomeToRef_Bwd_2Speeds = 13 + #eHomeToRef_Fwd_2Speeds = 14 + + eHomeToEncPulse_Bwd = 21 + eHomeToEncPulse_Fwd = 22 + eHomeToEncPulse_viaBwdLimit = 23 + eHomeToEncPulse_viaFwdLimit = 24 + + eHomeDirect = 90 + +class E_PneumaticAxisErrors(Enum): + eNoError = 0 + eExtractTimedOut = 1 + eRetractTimedOut = 2 + eNotMovingExtract = 3 + eNotMovingRetract = 4 + eInterlockOn = 5 + eNoPSSPermit = 6 + eAirPressureError = 7 + +# SLEEP_INTERVAL is the default that the software will sleep while waiting +# for a bit to change state +# Most if not all functions can specify this as an optional parameter if you +# want a different sleep time for one particular function. +# If not specified this is the default. +SLEEP_INTERVAL = 1 # s +MARGIN_OF_SAFETY = 2 +verboseMode = True +dateTimeObj = datetime.now() +prevPrintString = "Empty" + + +class plc: + # If running on Windows then TwinCAT should create a + # route for you already and thus senderIp and + # senderAmsNetId don't need to be provided + def __init__( + self, + plcAmsNetId, + plcPort, + plcIp=None, + senderAmsNetId=None, + senderIp=None, + hostname=None, + username="Administrator", + password="1", + connection=None + ): + + print("Constructor for PLC") + + self.plcAmsNetId = plcAmsNetId + self.plcPort = plcPort + self.plcIp = plcIp + self.senderAmsNetId = senderAmsNetId + self.senderIp = senderIp + self.hostname = hostname + self.username = username + self.password = password + self.connection = pyads.Connection(self.plcAmsNetId, self.plcPort) + + def __del__(self): + print("Destructor for PLC: Close connection") + self.connection.close() + + def connect(self): + print("Connect to PLC") + """ + platformIsLinux = pyads.utils.platform_is_linux() + print(f"isLinux()={platformIsLinux}") + if platformIsLinux: + if self.senderAmsNetId != None and self.senderIp != None: + pyads.add_route_to_plc( + self.senderAmsNetId, + # The second argument here should be hostname + # instead of senderIp but we prefer not to use + # hostname + self.senderIp, + self.plcIp, + self.username, + self.password, + route_name=self.hostname, + ) + else: + print( + "Either senderAmsNetId or senderIp set to none so assuming" + "the route has already been created" + ) + + local_port = pyads.open_port() + """ + self.connection.open() + print(f"is_open()={self.connection.is_open}") + + #pyads.set_local_address(self.senderAmsNetId) + print(f"get_local_address()={pyads.get_local_address()}") + + # If the connection was not successful this command will fail + print(f"read_device_info()={self.connection.read_device_info()}") + self.noOfAxes = self.connection.read_by_name( + "GVL_APP.nAXIS_NUM", pyads.PLCTYPE_INT + ) + print(f"GVL_APP.nAXIS_NUM={self.noOfAxes}") + + return self + # For reading and writing any variable you can use the pyads function of the plc: + # E.g.: plc_obj.connection.read_by_name("varName", pyads.PLCTYPE_XXX) + # E.g.: plc_obj.connection.write_by_name("varName", value, pyads.PLCTYPE_XXX) + +class axis: + def __init__(self, plcConnection, axisNum): + print("Constructor for axis") + self.plc = plcConnection + self.axisNum = axisNum + + def __del__(self): + print("Destructor for axis: Resetting jog commands") + self.jogStop() + + # Generic function for getting any variable on the Axis + def getGenericVariable(self, plcVarPath, plcVarType): + plcVarName = f"GVL.astAxes[{self.axisNum}].{plcVarPath}" + returnValue = self.plc.connection.read_by_name(plcVarName, plcVarType) + global prevPrintString + printString = f"{plcVarName}=: {returnValue}" + # if prevPrintString != printString: + # print(f"{dateTimeObj.now()} {printString}") + prevPrintString = printString + return returnValue + + # Get ST_Status variables + def getEnabledStatus(self): + return self.getGenericVariable("stStatus.bEnabled", pyads.PLCTYPE_BOOL) + + def getCommandAbortedStatus(self): + return self.getGenericVariable("stStatus.bCommandAborted", pyads.PLCTYPE_BOOL) + + def getBusyStatus(self): + return self.getGenericVariable("stStatus.bBusy", pyads.PLCTYPE_BOOL) + + def getDoneStatus(self): + return self.getGenericVariable("stStatus.bDone", pyads.PLCTYPE_BOOL) + + def getHomedStatus(self): + return self.getGenericVariable("stStatus.bHomed", pyads.PLCTYPE_BOOL) + + def getMovingStatus(self): + return self.getGenericVariable("stStatus.bMoving", pyads.PLCTYPE_BOOL) + + def getMovingFwdStatus(self): + return self.getGenericVariable("stStatus.bMovingForward", pyads.PLCTYPE_BOOL) + + def getMovingBwdStatus(self): + return self.getGenericVariable("stStatus.bMovingBackward", pyads.PLCTYPE_BOOL) + + def getFwdEnabled(self): + return self.getGenericVariable("stStatus.bFwEnabled", pyads.PLCTYPE_BOOL) + + def getBwdEnabled(self): + return self.getGenericVariable("stStatus.bBwEnabled", pyads.PLCTYPE_BOOL) + + def getInterlockedFwd(self): + return self.getGenericVariable("stStatus.bInterlockedFwd", pyads.PLCTYPE_BOOL) + + def getInterlockedBwd(self): + return self.getGenericVariable("stStatus.bInterlockedBwd", pyads.PLCTYPE_BOOL) + + def getInTargetPosition(self): + return self.getGenericVariable("stStatus.bInTargetPosition", pyads.PLCTYPE_BOOL) + + def getGearedStatus(self): + return self.getGenericVariable("stStatus.bGeared", pyads.PLCTYPE_BOOL) + + def getCoupledGear1(self): + return self.getGenericVariable("stStatus.bCoupledGear1", pyads.PLCTYPE_BOOL) + + def getCoupledGear2(self): + return self.getGenericVariable("stStatus.bCoupledGear2", pyads.PLCTYPE_BOOL) + + def getCoupledGear3(self): + return self.getGenericVariable("stStatus.bCoupledGear3", pyads.PLCTYPE_BOOL) + + def getCoupledGear4(self): + return self.getGenericVariable("stStatus.bCoupledGear4", pyads.PLCTYPE_BOOL) + + def getActPos(self): + return self.getGenericVariable("stStatus.fActPosition", pyads.PLCTYPE_LREAL) + + def getActVel(self): + return self.getGenericVariable("stStatus.fActVelocity", pyads.PLCTYPE_LREAL) + + def getErrorStatus(self): + return self.getGenericVariable("stStatus.bError", pyads.PLCTYPE_BOOL) + + def getErrorId(self): + return self.getGenericVariable("stStatus.nErrorID", pyads.PLCTYPE_UDINT) + + #Status of the ST_AxisStatus of the AXIS_REF + def getConstantVelocityStatus(self): + return self.getGenericVariable("Axis.Status.ConstantVelocity", pyads.PLCTYPE_BOOL) + + def getAcceleratingStatus(self): + return self.getGenericVariable("Axis.Status.Accelerating", pyads.PLCTYPE_BOOL) + + def getDeceleratingStatus(self): + return self.getGenericVariable("Axis.Status.Decelerating", pyads.PLCTYPE_BOOL) + + def getStandstillStatus(self): + return self.getGenericVariable("Axis.Status.Standstill", pyads.PLCTYPE_BOOL) + + # Get ST_Config variables + def getHomeSequence(self): + return self.getGenericVariable("stConfig.eHomeSeq", pyads.PLCTYPE_INT) + + def getHomePosition(self): + return self.getGenericVariable("stConfig.fHomePosition", pyads.PLCTYPE_LREAL) + + def getHomeFinishDistance(self): + return self.getGenericVariable("stConfig.fHomeFinishDistance", pyads.PLCTYPE_LREAL) + + def getVelocity(self): + return self.getGenericVariable("stControl.fVelocity", pyads.PLCTYPE_LREAL) + + def getAcceleration(self): + return self.getGenericVariable("stControl.fAcceleration", pyads.PLCTYPE_LREAL) + + def getDeceleration(self): + return self.getGenericVariable("stControl.fDeceleration", pyads.PLCTYPE_LREAL) + + def getPosition(self): + return self.getGenericVariable("stControl.fPosition", pyads.PLCTYPE_LREAL) + + def getMultiMasterAxis(self, masterIndex): + return self.getGenericVariable( + f"stConfig.astMultiMasterAxis[{masterIndex}].nIndex", pyads.PLCTYPE_UINT) + + def getMultiMasterRatio(self, masterIndex): + return self.getGenericVariable( + f"stConfig.astMultiMasterAxis[{masterIndex}].fRatio", pyads.PLCTYPE_LREAL) + + def getMultiMasterAxisLatched(self, masterIndex): + return self.getGenericVariable( + f"stConfig.astMultiMasterAxisLatched[{masterIndex}].nIndex", + pyads.PLCTYPE_UINT) + + def getMultiMasterRatioLatched(self, masterIndex): + return self.getGenericVariable( + f"stConfig.astMultiMasterAxisLatched[{masterIndex}].fRatio", + pyads.PLCTYPE_LREAL) + + def getMultiSlaveAxisRatio(self, slaveNum): + return self.getGenericVariable( + f"stConfig.afMultiSlaveAxisRatio[{slaveNum}]", pyads.PLCTYPE_LREAL) + + def getVelocityHomeToCam(self): + return self.getGenericVariable("stConfig.fHomingVelToCam", pyads.PLCTYPE_LREAL) + + def getVelocityHomeFromCam(self): + return self.getGenericVariable( + "stConfig.fHomingVelFromCam", pyads.PLCTYPE_LREAL) + + def getVelocityMax(self): + return self.getGenericVariable("stConfig.fVeloMax", pyads.PLCTYPE_LREAL) + + def getAccelMax(self): + return self.getGenericVariable("stConfig.fMaxAcc", pyads.PLCTYPE_LREAL) + + def getDecelMax(self): + return self.getGenericVariable("stConfig.fMaxDec", pyads.PLCTYPE_LREAL) + + def getSoftLimitFwdValue(self): + return self.getGenericVariable("stConfig.fMaxSoftPosLimit", pyads.PLCTYPE_LREAL) + + def getSoftLimitBwdValue(self): + return self.getGenericVariable("stConfig.fMinSoftPosLimit", pyads.PLCTYPE_LREAL) + + def getSoftLimitFwdEnableStatus(self): + return self.getGenericVariable( + "stConfig.bEnMaxSoftPosLimit", pyads.PLCTYPE_BOOL) + + def getSoftLimitBwdEnableStatus(self): + return self.getGenericVariable( + "stConfig.bEnMinSoftPosLimit", pyads.PLCTYPE_BOOL) + + def getAxisVeloManFast(self): + return self.getGenericVariable( + "stConfig.fVelocityDefaultFast", pyads.PLCTYPE_LREAL) + + def getAxisVeloManSlow(self): + return self.getGenericVariable( + "stConfig.fVelocityDefaultSlow", pyads.PLCTYPE_LREAL) + + def getAxisEnPositionLagMonitoring(self): + return self.getGenericVariable( + "stConfig.bEnPositionLagMonitoring", pyads.PLCTYPE_BOOL) + + def getAxisPositionLagValue(self): + return self.getGenericVariable("stConfig.fMaxPosLagValue", pyads.PLCTYPE_LREAL) + + def getAxisEnTargetPositionMonitoring(self): + return self.getGenericVariable( + "stConfig.bEnTargetPositionMonitoring", pyads.PLCTYPE_BOOL + ) + + def getAxisTargetPositionWindow(self): + return self.getGenericVariable( + "stConfig.fTargetPositionWindow", pyads.PLCTYPE_LREAL + ) + + + # Get ST_Input variables + def getLimitFwd(self): + return self.getGenericVariable("stInputs.bLimitFwd", pyads.PLCTYPE_BOOL) + + def getLimitBwd(self): + return self.getGenericVariable("stInputs.bLimitBwd", pyads.PLCTYPE_BOOL) + + def getHomeSwitch(self): + return self.getGenericVariable("stInputs.bHome", pyads.PLCTYPE_BOOL) + + # Generic function for setting any variable on the plc + def setGenericVariable(self, plcVarPath, plcVarValue, plcVarType): + plcVarName = f"GVL.astAxes[{self.axisNum}].{plcVarPath}" + print(f"{dateTimeObj.now()} {plcVarName}={plcVarValue}") + self.plc.connection.write_by_name(plcVarName, plcVarValue, plcVarType) + + def defineAsSlave(self): + return setGenericVariable("Axis.NcToPlc.CoupleState", 3, pyads.PLCTYPE_UDINT) + + def defineAsMaster(self): + return setGenericVariable("Axis.NcToPlc.CoupleState", 1, pyads.PLCTYPE_UDINT) + + # Set ST_Control variables + def executeAxis(self): + self.setGenericVariable("stControl.bExecute", True, pyads.PLCTYPE_BOOL) + + def resetAxis(self): + self.setGenericVariable("stControl.bReset", True, pyads.PLCTYPE_BOOL) + + def haltAxis(self): + self.setGenericVariable("stControl.bHalt", True, pyads.PLCTYPE_BOOL) + + def stopAxis(self): + self.setGenericVariable("stControl.bStop", True, pyads.PLCTYPE_BOOL) + + def enableAxis(self): + self.setGenericVariable("stControl.bEnable", True, pyads.PLCTYPE_BOOL) + + def disableAxis(self): + self.setGenericVariable("stControl.bEnable", False, pyads.PLCTYPE_BOOL) + + def setMotionCommand(self, command): #Called by the functions regarding a move + plcVarName = f"GVL.astAxes[{self.axisNum}].stControl.eCommand" + print(f"{dateTimeObj.now()} {plcVarName}={command.name}") + self.plc.connection.write_by_name(plcVarName, command.value, pyads.PLCTYPE_INT) + + def setVelocity(self, value): + self.setGenericVariable("stControl.fVelocity", value, pyads.PLCTYPE_LREAL) + + def setJogVelocity(self, value): + self.setGenericVariable("stControl.fJogVelocity", value, pyads.PLCTYPE_LREAL) + + def setAcceleration(self, value): + self.setGenericVariable("stControl.fAcceleration", value, pyads.PLCTYPE_LREAL) + + def setDeceleration(self, value): + self.setGenericVariable("stControl.fDeceleration", value, pyads.PLCTYPE_LREAL) + + def setPosition(self, value): + self.setGenericVariable("stControl.fPosition", value, pyads.PLCTYPE_LREAL) + + # Set ST_Config variables + def setOverride(self, value): + self.setGenericVariable("stConfig.fOveride", value, pyads.PLCTYPE_LREAL) + + def setHomePosition(self, value): + self.setGenericVariable("stConfig.fHomePosition", value, pyads.PLCTYPE_LREAL) + + def setHomeFinishDistance(self, value): + self.setGenericVariable("stConfig.fHomeFinishDistance", value, pyads.PLCTYPE_LREAL) + + def setHomeSequence(self, sequence): #Called by the function home() and homeSpecific() + plcVarName = f"GVL.astAxes[{self.axisNum}].stConfig.eHomeSeq" + print(f"{dateTimeObj.now()} {plcVarName}={sequence.name}") + self.plc.connection.write_by_name(plcVarName, sequence.value, pyads.PLCTYPE_INT) + + def setMultiMasterAxis(self, masterNum, masterAxisNum, gearRatio): + self.setGenericVariable( + f"stConfig.astMultiMasterAxis[{masterNum}].nIndex", + masterAxisNum, + pyads.PLCTYPE_UINT) + self.setGenericVariable( + f"stConfig.astMultiMasterAxis[{masterNum}].fRatio", + gearRatio, + pyads.PLCTYPE_LREAL) + + # Set some Nc Parameters that aren't exposed + def setSoftLimitsOn(self): + axis.setNcAxisParam(self, E_AxisParameters.EnableLimitFwd, 1.0) + axis.setNcAxisParam(self, E_AxisParameters.EnableLimitBwd, 1.0) + + def setSoftLimitsOff(self): + axis.setNcAxisParam(self, E_AxisParameters.EnableLimitFwd, 0.0) + axis.setNcAxisParam(self, E_AxisParameters.EnableLimitBwd, 0.0) + + def setFwdSoftLimitsOn(self): + axis.setNcAxisParam(self, E_AxisParameters.EnableLimitFwd, 1.0) + + def setBwdSoftLimitsOn(self): + axis.setNcAxisParam(self, E_AxisParameters.EnableLimitBwd, 1.0) + + def setFwdSoftLimitsOff(self): + axis.setNcAxisParam(self, E_AxisParameters.EnableLimitFwd, 0.0) + + def setBwdSoftLimitsOff(self): + axis.setNcAxisParam(self, E_AxisParameters.EnableLimitBwd, 0.0) + + def setSoftLimitFwdValue(self, softLimitFwdValue): + axis.setNcAxisParam(self, E_AxisParameters.SWLimitFwd, softLimitFwdValue) + + def setSoftLimitBwdValue(self, softLimitBwdValue): + axis.setNcAxisParam(self, E_AxisParameters.SWLimitBwd, softLimitBwdValue) + + def setAxisVeloManSlow(self, AxisVeloManSlow): + axis.setNcAxisParam(self, E_AxisParameters.AxisVeloManSlow, AxisVeloManSlow) + + def setAxisEnTargetPositionMonitoringON(self): + axis.setNcAxisParam(self, E_AxisParameters.AxisEnTargetPositionMonitoring, 1.0) + + def setAxisEnTargetPositionMonitoringOFF(self): + axis.setNcAxisParam(self, E_AxisParameters.AxisEnTargetPositionMonitoring, 0.0) + + def setAxisTargetPositionWindow(self, axisTargetPositionWindow): + axis.setNcAxisParam( + self, E_AxisParameters.AxisTargetPositionWindow, axisTargetPositionWindow + ) + + ###Motion commands### + def moveAbsolute(self, position): + print(f"Axis {self.axisNum}: Move absolute to position {position:.2f}") + self.setPosition(position) + self.setMotionCommand(E_MotionFunctions.eMoveAbsolute) + self.executeAxis() + + def moveAbsoluteAndWait(self, position): + self.moveAbsolute(position) + timeout = self.calcTravelTimeForMove()+1 + self.waitForCommandDone(timeoutDoneTrue=timeout) + return self.getDoneStatus() + + def moveRelative(self, position): + print(f"Axis {self.axisNum}: Move relative to position {position:.2f}") + self.setPosition(position) + self.setMotionCommand(E_MotionFunctions.eMoveRelative) + self.executeAxis() + + def moveRelativeAndWait(self, position): + self.moveRelative(position) + timeout = self.calcTravelTimeForMove()+1 + self.waitForCommandDone(timeoutDoneTrue=timeout) + return self.getDoneStatus() + + def jogFwd(self): + #self.setMotionCommand(E_MotionFunctions.eJog) + #self.setGenericVariable("stControl.bJogFwd", True, pyads.PLCTYPE_BOOL) + self.setVelocity(self.getAxisVeloManSlow()) + self.setMotionCommand(E_MotionFunctions.eMoveVelocity) + self.executeAxis() + + def jogBwd(self): + #self.setMotionCommand(E_MotionFunctions.eJog) + #self.setGenericVariable("stControl.bJogBwd", True, pyads.PLCTYPE_BOOL) + self.setVelocity(-(self.getAxisVeloManSlow())) + self.setMotionCommand(E_MotionFunctions.eMoveVelocity) + self.executeAxis() + + def jogStop(self): + #self.setGenericVariable("stControl.bJogFwd", False, pyads.PLCTYPE_BOOL) + #self.setGenericVariable("stControl.bJogBwd", False, pyads.PLCTYPE_BOOL) + self.haltAxis() + + def moveVelocity(self, velocity): + print(f"Axis {self.axisNum}: Move velocity with speed {velocity:.2f}") + self.setVelocity(velocity) + self.setMotionCommand(E_MotionFunctions.eMoveVelocity) + self.executeAxis() + + def moveToSwitchFwd(self, velo, timeout): + print(f" Activate moving to Forward Limit Switch sequence...") + if self.getSoftLimitFwdEnableStatus(): + print(' Disabling soft limits forward...') + self.setFwdSoftLimitsOff() + time.sleep(SLEEP_INTERVAL) + self.setBwdSoftLimitsOff() + time.sleep(SLEEP_INTERVAL) + if self.getSoftLimitFwdEnableStatus() or self.getSoftLimitBwdEnableStatus(): + print(f' Error: Failed to disable soft limits') + return False + + else: + print(f" Soft limits disabled, starting movement") + + if not self.getLimitFwd(): + print(" Axis possibly on the limit switch, moving away of it...") + for i in range(3): + if self.moveRelativeAndWait(-3): + time.sleep(1) + if self.getLimitFwd(): + print(" Axis not on the limit switch anymore...") + break + else: + i += 1 + if i == 3: + print(" ERROR: Timeout. Possibly limit without Power") + return False + + print(' Moving to Fwd Swtich...') + time.sleep(SLEEP_INTERVAL) + self.moveVelocity(velo) + + if self.waitForStatusBit(self.getLimitFwd, False, timeout): + print(' Fwd Limit reached...') + self.haltAxis() + time.sleep(SLEEP_INTERVAL) + return True + else: + print(f' ERROR: Axis {self.axisNum}: Timeout error waiting for LimitFwd to return False') + self.haltAxis() + return False + + def moveToSwitchBwd(self, velo, timeout): + if velo is None: + velo = self.getJogVelocity() + + print(f" Activate moving to Backward Limit Switch sequence...") + if self.getSoftLimitFwdEnableStatus() or self.getSoftLimitBwdEnableStatus(): + print(' Disabling soft limits...') + self.setFwdSoftLimitsOff() + time.sleep(SLEEP_INTERVAL) + self.setBwdSoftLimitsOff() + time.sleep(SLEEP_INTERVAL) + if self.getSoftLimitFwdEnableStatus() or self.getSoftLimitBwdEnableStatus(): + print(f' Error: Failed to disable soft limits') + return False + + else: + print(f" Soft limits disabled, starting movement") + + if not self.getLimitBwd(): + print(" Axis possibly on the limit switch, moving away of it...") + for i in range(3): + if self.moveRelativeAndWait(3): + time.sleep(1) + if self.getLimitBwd(): + print(" Axis not on the limit switch anymore...") + break + else: + i += 1 + if i == 3: + print(" ERROR: Timeout. Possibly limit without Power") + return False + + print(' Moving to Bwd Swtich...') + time.sleep(SLEEP_INTERVAL) + self.moveVelocity(-velo) + + if self.waitForStatusBit(self.getLimitBwd, False, timeout): + print(' Bwd Limit reached...') + self.haltAxis() + time.sleep(SLEEP_INTERVAL) + return True + else: + print(f' ERROR: Axis {self.axisNum}: Timeout error waiting for LimitFwd to return False') + self.haltAxis() + return False + + + def gearInMultiMaster( + self, + master1=None, + ratio1=None, + master2=None, + ratio2=None, + master3=None, + ratio3=None, + master4=None, + ratio4=None, + ): + print(f"Axis {self.axisNum}: Gear in multi master") + print(f"MasterAxis1 = {master1} with GearRatio1 = {ratio1}") + print(f"MasterAxis2 = {master2} with GearRatio2 = {ratio2}") + print(f"MasterAxis3 = {master3} with GearRatio3 = {ratio3}") + print(f"MasterAxis4 = {master4} with GearRatio4 = {ratio4}") + + self.setMotionCommand(E_MotionFunctions.eGearInMultiMaster) + if not master1 == None: + self.setMultiMasterAxis(1, master1, ratio1) + if not master2 == None: + self.setMultiMasterAxis(2, master2, ratio2) + if not master3 == None: + self.setMultiMasterAxis(3, master3, ratio3) + if not master4 == None: + self.setMultiMasterAxis(4, master4, ratio4) + + self.executeAxis() + + def gearOut(self): + print(f"Axis {self.axisNum}: Gear Out") + self.setMotionCommand(E_MotionFunctions.eGearOut) + self.executeAxis() + + def homeSpecific(self, homeSeq, homePos=0, homeFinishDist=0): + print(f"Axis {self.axisNum}: Home with HomeSeq={homeSeq}, HomePos={homePos}, HomeFinishDistance={homeFinishDist}") + self.setHomeSequence(E_HomingRoutines(homeSeq)) + self.setHomePosition(homePos) + self.setHomeFinishDistance(homeFinishDist) + self.setMotionCommand(E_MotionFunctions.eHome) + self.executeAxis() + + def home(self): + self.homeSeq = self.getHomeSequence() + self.homePos = self.getHomePosition() + self.homeFinishDist = self.getHomeFinishDistance() + print(f"Axis {self.axisNum}: Home with HomeSeq={self.homeSeq}, HomePos={self.homePos}, HomeFinishDistance={self.homeFinishDist}") + self.setMotionCommand(E_MotionFunctions.eHome) + self.executeAxis() + + def setNcAxisParam(self, axisParam, writeAxisParam): + self.setMotionCommand(E_MotionFunctions.eWriteParameter) + + plcVarName = f"GVL.astAxes[{self.axisNum}].stConfig.eAxisParameters" + print(f"{dateTimeObj.now()} {plcVarName}={axisParam.name}") + self.plc.connection.write_by_name( + plcVarName, axisParam.value, pyads.PLCTYPE_INT + ) + + plcVarName = f"GVL.astAxes[{self.axisNum}].stConfig.fWriteAxisParameter" + print(f"{dateTimeObj.now()} {plcVarName}={writeAxisParam}") + self.plc.connection.write_by_name( + plcVarName, writeAxisParam, pyads.PLCTYPE_LREAL + ) + + self.executeAxis() + time.sleep(0.05) + + def getNcAxisParam(self, axisParam): + self.setMotionCommand(E_MotionFunctions.eReadParameter) + + plcVarName = f"GVL.astAxes[{self.axisNum}].stConfig.eAxisParameters" + print(f"{dateTimeObj.now()} {plcVarName}={axisParam.name}") + self.plc.connection.write_by_name( + plcVarName, axisParam.value, pyads.PLCTYPE_INT + ) + self.executeAxis() + time.sleep(SLEEP_INTERVAL) + + plcVarName = f"GVL.astAxes[{self.axisNum}].stConfig.fReadAxisParameter" + readAxisParam = self.plc.connection.read_by_name( + plcVarName, pyads.PLCTYPE_LREAL + ) + print(f"{dateTimeObj.now()} {plcVarName}=?: {readAxisParam}") + + return readAxisParam + + ###Functions useful for testing### + + #This function disables, reset and enables the axis + def axisInit(self): + #Disable Axis + if self.getEnabledStatus(): + print(f" Disabling Axis...") + self.disableAxis() + time.sleep(1) + if self.waitForStatusBit(self.getEnabledStatus, False): + print(f" Axis disabled") + else: + print(" Axis disabled") + + #Reset Axis + print(' Resetting axis...') + self.resetAxis() + time.sleep(SLEEP_INTERVAL) + + #Enable Axis + if not self.getEnabledStatus(): + print(f" Enabling Axis...") + self.enableAxis() + time.sleep(SLEEP_INTERVAL) + if self.waitForStatusBit(self.getEnabledStatus, True): + print(f" Axis Enabled") + + def waitForVariable(self, varName, plcVarType, expectedValue, timeout=30, sleepInterval=SLEEP_INTERVAL): + # If timeout is negative time then just use a default + if timeout < 0: + timeout = 1 + + timeLimit = time.time() + timeout + timeoutError = False + while True: + variableValue=self.plc.connection.read_by_name(varName, plcVarType) + if str(variableValue) == str(expectedValue): + break + if time.time() > timeLimit: + timeoutError = True + break + if sleepInterval > 0: + time.sleep(sleepInterval) + + if timeoutError: + print( + f" Axis {self.axisNum}: Timeout error waiting for {varName} with value {variableValue} to return {expectedValue}" + ) + return False + else: + return True + + # boolValue is the status you're waiting for + # if you're waiting a bit to go high then this should be True + def waitForStatusBit( + self, getStatusBitFunction, boolValue, timeout=30, sleepInterval=SLEEP_INTERVAL + ): + # If timeout is negative time then just use a default + if timeout < 0: + timeout = 1 + + timeLimit = time.time() + timeout + timeoutError = False + while True: + statusBit = getStatusBitFunction() + if statusBit == boolValue: + break + if time.time() > timeLimit: + timeoutError = True + break + if sleepInterval > 0: + time.sleep(sleepInterval) + + if timeoutError: + print( + f" Axis {self.axisNum}: Timeout error waiting for {getStatusBitFunction.__name__} to return {boolValue}" + ) + return False + else: + return True + + def waitForCommandAborted(self): + return self.waitForStatusBit(self.getCommandAbortedStatus, True) + + # This ones a bit different to the previous generic waitForStatusBit + # It waits for bDone to go low, then bBusy high, then bDone high + def waitForCommandDone( + self, + timeoutDoneFalse=5, + timeoutBusyTrue=5, + timeoutDoneTrue=30, + sleepInterval=SLEEP_INTERVAL, + ): + if not self.waitForStatusBit( + self.getDoneStatus, + False, + timeout=timeoutDoneFalse, + sleepInterval=sleepInterval, + ): + print( + f" Axis {self.axisNum} Error: bDone status did not go low within {timeoutDoneFalse} seconds" + ) + return False + if not self.waitForStatusBit( + self.getBusyStatus, + True, + timeout=timeoutBusyTrue, + sleepInterval=sleepInterval, + ): + print( + f" Axis {self.axisNum} Error: bBusy status did not go high within {timeoutBusyTrue} seconds" + ) + return False + if not self.waitForStatusBit( + self.getDoneStatus, + True, + timeout=timeoutDoneTrue, + sleepInterval=sleepInterval, + ): + print( + f" Axis {self.axisNum} Error: bDone status did not go high within {timeoutDoneTrue} seconds" + ) + return False + return True + + # This one is also a bit special as I don't think we currently have a + # stop bit in the ast.axisStruct + # Therefore this function checks the actual velocity is 0 + def waitForStop( + self, timeout=30, sleepInterval=SLEEP_INTERVAL, roundVelDecimalPlaces=2 + ): + # If timeout is negative time then just use a default + if timeout < 0: + timeout = 1 + + timeLimit = time.time() + timeout + bTimeoutError = False + while True: + if round(self.getActVel(), roundVelDecimalPlaces) == 0 or not self.getMovingStatus(): + break + if time.time() > timeLimit: + bTimeoutError = True + break + if sleepInterval > 0: + time.sleep(sleepInterval) + + if bTimeoutError: + print( + f" Axis {self.axisNum} Error: Timeout of {timeout} exceeded waiting for velocity to be zero" + ) + return False + else: + return True + + # Check that the position is within the specificed range post move + def checkTargetPositionWindow(self, targetPos=None): + # Not sure if getPos is the best variable, this is just what's in the + # axisStuct and not the setPos in the NC + actPos = self.getActPos() + if targetPos == None: + targetPos = self.getPosition() + targetPosWindow = self.getAxisTargetPositionWindow() + + error = abs(actPos - targetPos) + + print( + f"Set Pos: {targetPos:.2f}, Act Pos: {actPos:.2f}, Error: {error:.2f}, Pos Window: {targetPosWindow:.2f}" + ) + + if error < targetPosWindow: + return True + else: + return False + + def calcTravelTimeForMove(self, marginOfSafety=MARGIN_OF_SAFETY): + print(f"Calculating expected travel time for current move") + + vel = self.getVelocity() + acc = self.getAcceleration() + dec = self.getDeceleration() + currentPos = self.getActPos() + finalPos = self.getPosition() + + print( + f"Vel={vel:.2f}, Acc={acc:.2f}, Dec={dec:.2f}, Curr Pos={currentPos:.2f},Final Pos={finalPos:.2f}" + ) + + timeToAcc = abs(vel / acc) + timeToDec = abs(vel / dec) + + accDist = 0.5 * acc * timeToAcc * timeToAcc + decDist = 0.5 * acc * timeToDec * timeToDec + + distToTravel = abs(currentPos - finalPos) + + distForConstVel = distToTravel - accDist - decDist + + if vel == 0: + print(" Error: Can't divide by a velocity of 0") + return -1 + + timeAtConstVel = abs(distForConstVel / vel) + + totalTravelTime = timeToAcc + timeAtConstVel + timeToDec + + estTravelTime = totalTravelTime * marginOfSafety + + print( + f"Estimated time for the move is: {estTravelTime:.2f}s with a safety factor of {marginOfSafety}" + ) + + return estTravelTime + + def calcTravelTimeForPosition(self, finalPos, marginOfSafety=MARGIN_OF_SAFETY): + print(f"Calculating expected travel time for current move") + + vel = self.getVelocity() + acc = self.getAcceleration() + dec = self.getDeceleration() + currentPos = self.getActPos() + + print( + f"Vel={vel:.2f}, Acc={acc:.2f}, Dec={dec:.2f}, Curr Pos={currentPos:.2f},Final Pos={finalPos:.2f}" + ) + + timeToAcc = abs(vel / acc) + timeToDec = abs(vel / dec) + + accDist = 0.5 * acc * timeToAcc * timeToAcc + decDist = 0.5 * acc * timeToDec * timeToDec + + distToTravel = abs(currentPos - finalPos) + + distForConstVel = distToTravel - accDist - decDist + + if vel == 0: + print(" Error: Can't divide by a velocity of 0") + return -1 + + timeAtConstVel = abs(distForConstVel / vel) + + totalTravelTime = timeToAcc + timeAtConstVel + timeToDec + + estTravelTime = totalTravelTime * marginOfSafety + + print( + f"Estimated time for the move is: {estTravelTime:.2f}s with a safety factor of {marginOfSafety}" + ) + + return estTravelTime + + # This calculation uses the soft limits to calculate the max distance the + # axis would have to move. + # This distance divided by the homing speed estimates the max homing time + # and can thus be used for the timeout when homing + def calcTravelTimeForRange(self, marginOfSafety=MARGIN_OF_SAFETY): + print(f"Calculating expected travel time for axis range") + softLimFwd = self.getSoftLimitFwdValue() + softLimNeg = self.getSoftLimitBwdValue() + homingVelocity = self.getVelocityHomeFromCam() + + if (softLimFwd == 0 and softLimNeg == 0): + print(f' ERROR: No soft limits defined, cannot calculate travel range') + return -1 + + travelRange = abs(softLimFwd - softLimNeg) + + # Use 2 * travel range because if you start homing in the wrong direction + # you might need to move the whol range twice + # Use homing from cam as it's a slower speed + if homingVelocity == 0: + print(" Error: Can't divide by a homing velocity of 0") + return -1 + + totalTravelTime = abs(2 * travelRange / homingVelocity) + + print(f"Range={travelRange:.2f}, Homing Vel={homingVelocity:.2f}") + + estTravelTime = totalTravelTime * marginOfSafety + + print( + f"Estimated time for the axis range is: {estTravelTime:.2f}s with a safety factor of {marginOfSafety:.2f}" + ) + + return estTravelTime + + def calcTimeForAccel(self, marginOfSafety=MARGIN_OF_SAFETY): + print(f"Calculating expected travel time for axis acceleration") + + setVel = ( + self.getVelocityHomeFromCam() + ) # perhaps should be changed as it could be to cam as well + accel = self.getAcceleration() + + if accel == 0: + print(" Error: Can't divide by an acceleration of 0") + return -1 + + timeForAccel = abs(setVel / accel) + + print(f"Set Velocity={setVel:.2f}, Acceleration={accel:.2f}") + + estTravelTime = timeForAccel * marginOfSafety + + print( + f"Estimated Acceleration time for the axis is: {estTravelTime:.2f}s with a safety factor of {marginOfSafety:.2f}" + ) + + return estTravelTime + + def calcTimeForDecel(self, marginOfSafety=MARGIN_OF_SAFETY): + print(f"Calculating expected travel time for axis deceleration") + + # Since this is just used for timeouts mostly I've used max vel + # instead of actVel as there were issues with delays and actVel + # not always being accurate + actVel = self.getVelocityMax() + decel = self.getDeceleration() + + if decel == 0: + print(" Error: Can't divide by a deceleration of 0") + return -1 + + timeForDecel = abs(actVel / decel) + + print(f"Act Velocity={actVel:.2f}, Deceleration={decel:.2f}") + + estTravelTime = timeForDecel * marginOfSafety + + print( + f"Estimated deceleration time for the axis is: {estTravelTime:.2f}s with a safety factor of {marginOfSafety:.2f}" + ) + + return estTravelTime + +class PneumaticAxis: + def __init__(self, plcConnection, axisNum): + print("Constructor for axis") + self.plc = plcConnection + self.axisNum = axisNum + + def __del__(self): + print("Destructor for pneumatic axis: going to fail safe state") + self.setValveOff() + + # Generic function for getting any variable on the pneumatic axis + def getGenericVariable(self, plcVarPath, plcVarType): + plcVarName = f"GVL.astPneumaticAxes[{self.axisNum}].{plcVarPath}" + returnValue = self.plc.connection.read_by_name(plcVarName, plcVarType) + global prevPrintString + printString = f"{plcVarName}=: {returnValue}" + if prevPrintString != printString: + print(f"{dateTimeObj.now()} {printString}") + prevPrintString = printString + return returnValue + + # Get ST_PneumaticAxisStatus variables + def getExtendingStatus(self): + return self.getGenericVariable("stPneumaticAxisStatus.bExtending", pyads.PLCTYPE_BOOL) + + def getRetractingStatus(self): + return self.getGenericVariable("stPneumaticAxisStatus.bRetracting", pyads.PLCTYPE_BOOL) + + def getExtendedStatus(self): + return self.getGenericVariable("stPneumaticAxisStatus.bExtended", pyads.PLCTYPE_BOOL) + + def getRetractedStatus(self): + return self.getGenericVariable("stPneumaticAxisStatus.bRetracted", pyads.PLCTYPE_BOOL) + + def getSolenoidActiveStatus(self): + return self.getGenericVariable("stPneumaticAxisStatus.bSolenoidActive", pyads.PLCTYPE_BOOL) + + def getInterlockedStatus(self): + return self.getGenericVariable("stPneumaticAxisStatus.bInterlocked", pyads.PLCTYPE_BOOL) + + def getPSSPermitOKStatus(self): + return self.getGenericVariable("stPneumaticAxisStatus.bPSSPermitOK", pyads.PLCTYPE_BOOL) + + def getErrorStatus(self): + return self.getGenericVariable("stPneumaticAxisStatus.bError", pyads.PLCTYPE_BOOL) + + def getTimeElapsedExtend(self): + return self.getGenericVariable("stPneumaticAxisStatus.nTimeElapsedExtend", pyads.PLCTYPE_INT) + + def getTimeElapsedRetract(self): + return self.getGenericVariable("stPneumaticAxisStatus.nTimeElapsedRetract", pyads.PLCTYPE_INT) + + def getStatus(self): + return self.getGenericVariable("stPneumaticAxisStatus.sStatus", pyads.PLCTYPE_STRING) + + # Get ST_PneumaticAxisConfig variables + def getTimeToExtend(self): + return self.getGenericVariable("stPneumaticAxisConfig.nTimeToExtend", pyads.PLCTYPE_INT) + + def getTimeToRetract(self): + return self.getGenericVariable("stPneumaticAxisConfig.nTimeToRetract", pyads.PLCTYPE_INT) + + # Get ST_PneumaticAxisInputs variables + def getEndSwitchFwd(self): + return self.getGenericVariable("stPneumaticAxisInputs.bEndSwitchFwd", pyads.PLCTYPE_BOOL) + + def getEndSwitchBwd(self): + return self.getGenericVariable("stPneumaticAxisInputs.bEndSwitchBwd", pyads.PLCTYPE_BOOL) + + def getSolenoidActive(self): + return self.getGenericVariable("stPneumaticAxisInputs.bSolenoidActive", pyads.PLCTYPE_BOOL) + + def getPSSPermit(self): + return self.getGenericVariable("stPneumaticAxisInputs.bPSSPermit", pyads.PLCTYPE_BOOL) + + def getPressureExtend(self): + return self.getGenericVariable("stPneumaticAxisInputs.bPressureExtend", pyads.PLCTYPE_BOOL) + + def getPressureRetract(self): + return self.getGenericVariable("stPneumaticAxisInputs.bPressureRetract", pyads.PLCTYPE_BOOL) + + def getOpenManual(self): + return self.getGenericVariable("stPneumaticAxisInputs.bOpenManual", pyads.PLCTYPE_BOOL) + + def getCloseManual(self): + return self.getGenericVariable("stPneumaticAxisInputs.bCloseManual", pyads.PLCTYPE_BOOL) + + def getAirPressureValve(self): + return self.getGenericVariable("stPneumaticAxisInputs.nAirPressureValve", pyads.PLCTYPE_INT) + + def getPressureValue(self): + return self.getGenericVariable("stPneumaticAxisInputs.nPressureValue", pyads.PLCTYPE_INT) + + # Get ST_PneumaticAxisOutputs variables + def getValveState(self): + return self.getGenericVariable("stPneumaticAxisOutputs.bValveOn", pyads.PLCTYPE_BOOL) + + def getAirPressureOnState(self): + return self.getGenericVariable("stPneumaticAxisOutputs.bAirPressureOn", pyads.PLCTYPE_BOOL) + + # Generic function for setting any variable on the plc + def setGenericVariable(self, plcVarPath, plcVarValue, plcVarType): + plcVarName = f"GVL.astPneumaticAxes[{self.axisNum}].{plcVarPath}" + print(f"{dateTimeObj.now()} {plcVarName}={plcVarValue}") + self.plc.connection.write_by_name(plcVarName, plcVarValue, plcVarType) + + # Set ST_PneumaticAxisControl variables + def extendPneumaticAxis(self): + self.setGenericVariable("stPneumaticAxisControl.bExtend", True, pyads.PLCTYPE_BOOL) + + def retractPneumaticAxis(self): + self.setGenericVariable("stPneumaticAxisControl.bRetract", True, pyads.PLCTYPE_BOOL) + + def interlockPneumaticAxis(self): + self.setGenericVariable("stPneumaticAxisControl.bInterlock", True, pyads.PLCTYPE_BOOL) + + def resetPneumaticAxis(self): + self.setGenericVariable("stPneumaticAxisControl.bReset", True, pyads.PLCTYPE_BOOL) + + # Set ST_PneumaticAxisConfig variables + def setTimeToExtend(self, value): + return self.setGenericVariable("stPneumaticAxisConfig.nTimeToExtend", value, pyads.PLCTYPE_INT) + + def setTimeToRetract(self, value): + return self.setGenericVariable("stPneumaticAxisConfig.nTimeToRetract", value, pyads.PLCTYPE_INT) + + # Set ST_PneumaticAxisOutputs variables + def setValveOn(self): + self.setGenericVariable("stPneumaticAxisOutputs.bValveOn", True, pyads.PLCTYPE_BOOL) + + def setValveOff(self): + self.setGenericVariable("stPneumaticAxisOutputs.bValveOn", False, pyads.PLCTYPE_BOOL) + + ###Motion commands### + def extendAndWait(self): + timeout = self.getTimeToExtend() + self.extendPneumaticAxis() + self.waitForExtended(timeoutExtended=timeout) + return self.getExtendedStatus() + + def retractAndWait(self): + timeout = self.getTimeToRetract() + self.retractPneumaticAxis() + self.waitForRetracted(timeoutRetracted=timeout) + return self.getRetractedStatus() + + def ValveOffAndWait(self, timeoutMovement): + self.setValveOff() + self.waitForValveStateChange(timeoutMovementDone=timeoutMovement) + return self.getValveState() + + def ValveONAndWait(self, timeoutMovement): + self.setValveOn() + self.waitForValveStateChange(timeoutMovementDone=timeoutMovement) + return self.getValveState() + + # boolValue is the status you're waiting for + # if you're waiting a bit to go high then this should be True + def waitForStatusBit( + self, getStatusBitFunction, boolValue, timeout=30, sleepInterval=SLEEP_INTERVAL + ): + # If timeout is negative time then just use a default + if timeout < 0: + timeout = 1 + + timeLimit = time.time() + timeout + timeoutError = False + while True: + statusBit = getStatusBitFunction() + if statusBit == boolValue: + break + if time.time() > timeLimit: + timeoutError = True + break + if sleepInterval > 0: + time.sleep(sleepInterval) + + if timeoutError: + print( + f" Axis {self.axisNum}: Timeout error waiting for {getStatusBitFunction.__name__} to return {boolValue}" + ) + return False + else: + return True + + def waitForExtended(self, + timeoutExtended=30, + timeoutRetractedFalse=3, + timeoutExtending=3, + sleepInterval=SLEEP_INTERVAL): + + if not self.waitForStatusBit( + self.getRetractedStatus, + False, + timeout=timeoutRetractedFalse, + sleepInterval=sleepInterval, + ): + print( + f" Axis {self.axisNum} Error: bRetracted status did not go low within {timeoutRetractedFalse} seconds" + ) + return False + if not self.waitForStatusBit( + self.getExtendingStatus, + True, + timeout=timeoutExtending, + sleepInterval=sleepInterval, + ): + print( + f" Axis {self.axisNum} Error: bExtending status did not go high within {timeoutExtending} seconds" + ) + return False + if not self.waitForStatusBit( + self.getExtendedStatus, + True, + timeout=timeoutExtended, + sleepInterval=sleepInterval, + ): + print( + f" Axis {self.axisNum} Error: bExtended status did not go high within {timeoutExtended} seconds" + ) + return False + return True + + def waitForRetracted(self, + timeoutRetracted=30, + timeoutExtendedFalse=3, + timeoutRetracting=3, + sleepInterval=SLEEP_INTERVAL): + + if not self.waitForStatusBit( + self.getExtendedStatus, + False, + timeout=timeoutExtendedFalse, + sleepInterval=sleepInterval, + ): + print( + f" Axis {self.axisNum} Error: bExtended status did not go low within {timeoutExtendedFalse} seconds" + ) + return False + if not self.waitForStatusBit( + self.getRetractingStatus, + True, + timeout=timeoutRetracting, + sleepInterval=sleepInterval, + ): + print( + f" Axis {self.axisNum} Error: bRetracting status did not go high within {timeoutRetracting} seconds" + ) + return False + if not self.waitForStatusBit( + self.getRetractedStatus, + True, + timeout=timeoutRetracted, + sleepInterval=sleepInterval, + ): + print( + f" Axis {self.axisNum} Error: bRetracted status did not go high within {timeoutRetracted} seconds" + ) + return False + return True + + def waitForSwitchStateChange(self, + timeoutMovementDone=30, + timeoutEndSwitchOff=3, + timeoutMoving=3, + sleepInterval=SLEEP_INTERVAL): + + if self.getEndSwitchBwd(): + if not self.waitForStatusBit( + self.getEndSwitchBwd, + False, + timeout=timeoutEndSwitchOff, + sleepInterval=sleepInterval, + ): + print( + f" Axis {self.axisNum} Error: bEndSwitchBwd status did not go low within {timeoutEndSwitchOff} seconds" + ) + return False + + if not self.waitForStatusBit( + self.getEndSwitchFwd, + True, + timeout=timeoutMovementDone, + sleepInterval=sleepInterval, + ): + print( + f" Axis {self.axisNum} Error: bEndSwitchFwd status did not go high within {timeoutMovementDone} seconds" + ) + return False + return True + else: + if not self.waitForStatusBit( + self.getEndSwitchFwd, + False, + timeout=timeoutEndSwitchOff, + sleepInterval=sleepInterval, + ): + print( + f" Axis {self.axisNum} Error: bEndSwitchFwd status did not go low within {timeoutEndSwitchOff} seconds" + ) + return False + + if not self.waitForStatusBit( + self.getEndSwitchBwd, + True, + timeout=timeoutMovementDone, + sleepInterval=sleepInterval, + ): + print( + f" Axis {self.axisNum} Error: bEndSwitchBwd status did not go high within {timeoutMovementDone} seconds" + ) + return False + return True \ No newline at end of file