diff --git a/Scripts/Test_torque.py b/Scripts/Test_torque.py index b36ad6a..861fa27 100644 --- a/Scripts/Test_torque.py +++ b/Scripts/Test_torque.py @@ -7,15 +7,26 @@ import time import struct import struct -from ctypes import ( - memmove, - addressof, - c_ubyte, - Array, - Structure, - sizeof, - create_string_buffer, -) +MAX_SPEED = 7.1 #mm/s +MAX_POS =68 #mm +MIN_POS = 40 #mm +ACTIVE_AXIS = (0,1) +MASTER_AXIS = 0 +DEFAULT_SPEED = 7#mm/s + +#Limit function +def exeedingMaxVel(currVel): + print(f"selected velocity ({currVel}) exceeds maximum velocity ({MAX_SPEED})\n " + f"Warning move will be executed with the maximum speed of {MAX_SPEED}") + return MAX_SPEED + +def exeedingPosLimits(desiredPos): + print(f"selected pos of {desiredPos} exceeds position Limits ({MIN_POS}-{MAX_POS})\n " + "Aborting execution") + return exit(-1) + + + from datetime import datetime from functools import partial from typing import Optional, Union, Tuple, Any, Type, Callable, Dict, List, cast, OrderedDict @@ -46,38 +57,103 @@ plc = pyads.Connection(AMS_NET_ID, PORT,PLC_IP) def gearin(connection): print(connection.write_by_name("GVL.GerarInCmd", True, pyads.PLCTYPE_BOOL)) print(connection.write_by_name("GVL.GerarOutCmd", False, pyads.PLCTYPE_BOOL)) + time.sleep(1) + if connection.read_by_name("GVL.GeraredStat", pyads.PLCTYPE_BOOL) == False: + print("coupling failed, ABORTING") + exit(-1) + if connection.read_by_name("GVL.ErrOut", pyads.PLCTYPE_UDINT) != 0: + print("coupling failed with Error") + print(f"Axis Error Nr: {connection.read_by_name('GVL.ErrOut')}") + exit(-1) -def gearout(connection): +def gearout(connection: pyads.Connection): print(connection.write_by_name("GVL.GerarInCmd", False, pyads.PLCTYPE_BOOL)) print(connection.write_by_name("GVL.GerarOutCmd", True, pyads.PLCTYPE_BOOL)) + if connection.read_by_name("GVL.ErrOut", pyads.PLCTYPE_UDINT) != 0: + print("decouple failed with Error") + print(f"Axis Error Nr: {connection.read_by_name('GVL.ErrOut')}") + exit(-1) def reset(connection): connection.write_by_name("GVL.Reset",True, pyads.PLCTYPE_BOOL) + if connection.read_by_name("GVL.ErrOut", pyads.PLCTYPE_UDINT) != 0: + print("Reset Failed") + print(f"Axis Error Nr: {connection.read_by_name('GVL.ErrOut')}") + exit(-1) -def enableAxis(connection): +def enableAxis(connection: pyads.Connection): connection.write_by_name("GVL.EnableAxis", True, pyads.PLCTYPE_BOOL) + if connection.read_by_name("GVL.ErrOut",pyads.PLCTYPE_UDINT) != 0: + print("enableAxis failed") + print(f"Axis Error Nr: {connection.read_by_name('GVL.ErrOut')}") + exit(-1) + return 0 -def disableAxis(connection): +def disableAxis(connection: pyads.Connection): connection.write_by_name("GVL.EnableAxis", False, pyads.PLCTYPE_BOOL) + if connection.read_by_name("GVL.ErrOut", pyads.PLCTYPE_UDINT) != 0: + print("Disable Failed, Reseting Axis") + reset(connection) + print("Disable Axis") + connection.write_by_name("GVL.EnableAxis", False, pyads.PLCTYPE_BOOL) + print("Aborting with error:", connection.read_by_name("GVL.ErrOut", pyads.PLCTYPE_UDINT)) + exit(-1) -def stopAxis(connection): + +def stopAxis(connection: pyads.Connection): while connection.read_by_name("GVL.Axis1Stoping", pyads.PLCTYPE_BOOL) == False: connection.write_by_name("GVL.StopCmd", True, pyads.PLCTYPE_BOOL) + if connection.read_by_name("GVL.ErrOut", pyads.PLCTYPE_UDINT) != 0: + print("Stop Failed, disabling Axis") + print(f"Axis Error Nr: {connection.read_by_name('GVL.ErrOut')}") + disableAxis(connection) + print("ABORTING") + exit(-1) connection.write_by_name("GVL.StopCmd", False, pyads.PLCTYPE_BOOL) -def moveAbsolut(connection, Axis_ref, TargetPos, velocity): +def moveAbsolut(connection: pyads.Connection, Axis_ref, TargetPos, velocity): + # Checking prameters + check_position = (TargetPos if (MIN_POS <= TargetPos <= MAX_POS) else exeedingPosLimits(TargetPos)) + check_velocity = (velocity if ( velocity <= MAX_SPEED) else exeedingMaxVel(velocity)) + check_active_axis = (AXIS1_REF if Axis_ref in ACTIVE_AXIS else False) + print(check_active_axis) connection.write_by_name("GVL.moveInterface.targetPos", TargetPos,pyads.PLCTYPE_LREAL) connection.write_by_name("GVL.moveInterface.axisNr", Axis_ref, pyads.PLCTYPE_UINT) - connection.write_by_name("GVL.moveInterface.velocity", velocity, pyads.PLCTYPE_LREAL) + connection.write_by_name("GVL.moveInterface.velocity", check_velocity, pyads.PLCTYPE_LREAL) + if connection.read_by_name("GVL.ErrOut", pyads.PLCTYPE_UDINT) != 0: + print("Setting Parameters for absolut Move failed with Error") + print(f"Axis Error Nr: {connection.read_by_name('GVL.ErrOut')}") + exit(-1) + +def executeCmd(connection: pyads.Connection): -def executeCmd(connection): if connection.read_by_name("GVL.StartCmd", pyads.PLCTYPE_BOOL) == True: connection.write_by_name("GVL.StartCmd", False, pyads.PLCTYPE_BOOL) while connection.read_by_name("GVL.Done", pyads.PLCTYPE_BOOL) == False: connection.write_by_name("GVL.StartCmd", True, pyads.PLCTYPE_BOOL) + match connection.read_by_name("GVL.ErrOut", pyads.PLCTYPE_UDINT): + case 18000: + print("Emergency Stop pressed while executing move") + connection.close() + exit(-1) + case 17056: + print("limit switch triggered") + connection.close() + exit(-1) + case 0: + continue + case _: + print("Execute failed with Error\n Stopping Axis") + stopAxis(connection) + print("Disabling Axis") + disableAxis(connection) + print("ABORTING") + exit(-1) + + connection.write_by_name("GVL.Done", False, pyads.PLCTYPE_BOOL) connection.write_by_name("GVL.StartCmd", False, pyads.PLCTYPE_BOOL) -def cyclic_meas(connection, pos, wait,rep): +def cyclic_meas(connection: pyads.Connection, pos, wait,rep): disableAxis(connection) reset(connection) gearin(connection) @@ -87,7 +163,7 @@ def cyclic_meas(connection, pos, wait,rep): exit(-1) for i in range(rep): for x,t in zip(pos,wait): - moveAbsolut(connection, 0,x,10) + moveAbsolut(connection, MASTER_AXIS,x,DEFAULT_SPEED) executeCmd(connection) time.sleep(t) disableAxis(connection) @@ -96,19 +172,22 @@ def cyclic_meas(connection, pos, wait,rep): if __name__ == "__main__": - print("Testing torque") + print("Start measurement") plc.open() - #enableAxis(plc) + reset(plc) + gearin(plc) + enableAxis(plc) #reset(plc) #disableAxis(plc) #enableAxis(plc) - #gearout(plc) + #stopAxis(plc) #plc.write_by_name("GVL.StopCmd", False, pyads.PLCTYPE_BOOL) - #moveAbsolut(plc, 1, 100, 0.5) - #executeCmd(plc) - # - cyclic_meas(plc,[40,50],[10,50],200) - + moveAbsolut(plc, 0, 50, 0.1) + executeCmd(plc) + #cyclic_meas(plc,[46.085,56.085],[5,5],2) + #time.sleep(3) + disableAxis(plc) + gearout(plc) plc.close()