# Copyright 2015 - RoboDK Inc. - https://robodk.com/ # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # http://www.apache.org/licenses/LICENSE-2.0 # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------- # This file is a POST PROCESSOR for Robot Offline Programming to generate programs # for Annin Robotics (AR2 and AR3) # # To edit/test this POST PROCESSOR script file: # Select "Program"->"Add/Edit Post Processor", then select your post or create a new one. # You can edit this file using any text editor or Python editor. Using a Python editor allows to quickly evaluate a sample program at the end of this file. # Python should be automatically installed with RoboDK # # You can also edit the POST PROCESSOR manually: # 1- Open the *.py file with Python IDLE (right click -> Edit with IDLE) # 2- Make the necessary changes # 3- Run the file to open Python Shell: Run -> Run module (F5 by default) # 4- The "test_post()" function is called automatically # Alternatively, you can edit this file using a text editor and run it with Python # # To use a POST PROCESSOR file you must place the *.py file in "C:/RoboDK/Posts/" # To select one POST PROCESSOR for your robot in RoboDK you must follow these steps: # 1- Open the robot panel (double click a robot) # 2- Select "Parameters" # 3- Select "Unlock advanced options" # 4- Select your post as the file name in the "Robot brand" box # # To delete an existing POST PROCESSOR script, simply delete this file (.py file) # # ---------------------------------------------------- # More information about RoboDK Post Processors and Offline Programming here: # https://robodk.com/help#PostProcessor # https://robodk.com/doc/en/PythonAPI/postprocessor.html # ---------------------------------------------------- # ---------------------------------------------------- # Description # The Annin Robotics post processor allows for code generation (.ar3) for Annin Robotics controllers. # # # Supported Controllers # Annin Robotics AR2, Annin Robotics AR3 # ---------------------------------------------------- # ---------------------------------------------------- # TODO: Once the program is generated: Generate a Python module that can send the program to the robot (similar to Techman driver/post) # TODO: If we select Ctrl+F6 (Send program to robot): automatically trigger the execution of the external program (similar to Techman driver/post) # TODO: Automatically search for the right COM (similar to GAGE COM) # Nice to have controller updates # TODO: It would be nice to have a way to buffer data in the controller # TODO: It would be nice to support reading the text file as is on the controller side (support Move J and MJ) # TODO: Read data by pairs iteratively, after splitting the string (instead of using Indexof) # TODO: It would be nice to have a binary protocol that doesn't require parsing strings and includes a CRC check. Example: # 4 byte size + 4 byte command (EG: 1=MoveJ joints) + 4 bytes (joint size) + joint1 (8 bytes) + joint2 (8 bytes) + joint3 (8 bytes) + ... + jointN (8 bytes) # 4 byte size + 4 byte command (EG: 2=MoveL cartesian) + 4 bytes (joint size) + X (8 bytes) + Y (8 bytes) + Z (8 bytes) + ... + jointN (8 bytes) # Import RoboDK tools from robodk import * from robolink import import_install # Serial import_install('serial') import serial import pickle #----------------------------------------------- # Annin Robotics Serial communication class #----------------------------------------------- class SerialAR(): # Serial device com = None def __init__(self,port=None,timeout=0.2): # Default ports for open attempts comid_range = 20 test_ports = None comid_str = "" if port is list: test_ports = port for p in port: comid_str += " COM%i" % p elif port is not None: test_ports = [int(port)] comid_str = "COM%i" % port else: test_ports = range(comid_range) comid_str = "COM%i-COM%i" % (0, comid_range) # Try opening the port within the desired range for comid in test_ports: try: if sys.platform.startswith('linux'): # Ubuntu/Debian port_str = '/dev/ttyUSB%i'%comid else: # Windows: port_str = 'COM%i'%comid self.ser = serial.Serial(port=(port_str), baudrate=115200, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS, timeout=timeout ,write_timeout=timeout) except serial.serialutil.SerialException: #printLog(sys.exc_info()[0]) continue print("Trying to connect to robot through COM%i" % comid) if self.com.isOpen(): self.com.write(b'ID?\r') gage_name = self.com.read_until(b'\r') if gage_name.startswith(b'SY'): print("Found gage: " + str(gage_name.strip())) break else: print("Unknown gage or serial device! ID returned: " + str(gage_name)) self.com = None continue if self.com is None: msg = "Unable to connect to robot: No Serial/COM port detected (tested %s). Is the robot connected? Is it already being used?" % comid_str raise Exception(msg) def Close(self): if self.ser is not None: self.ser.close() self.ser = None def MoveJ(self, joints): """Take a measurement directly from the gage""" str_send = 'MJ' data = ['A','B','C','D','E','F','G'] for i in range(len(joints)): str_send = str_send + (' %s %.5f' % (data[i], joints[i])) self.ser.write(bytes(str_send,'utf-8')) #time.sleep(0.001) errorcode = self.ser.read_until(b'\r') if errorcode != "00": print("Error: " + errorcode) return False return True #trycount = 0 ## Sometimes we receive just \r #while not strvalue.endswith(b'\r') or len(strvalue) < 2 or not (strvalue[0] == 43) or not strvalue[1:-1].isascii() or not str(strvalue[1:-1],'utf-8').replace('.','').isnumeric():# '+' == 43 or strvalue[0] == b'-'): # printLog("%% Gage timed out, received: " + str(strvalue)) # self.gage.write(b'?\r') # strvalue = self.gage.read_until(b'\r') # trycount += 1 # if trycount > 20: # raise Exception("The device did not respond after %i attempts" % trycount) #return float(strvalue)#.strip()) #def Version(self): # self.gage.write(b'VER?\r') # return self.gage.read_until(b'\r') # ---------------------------------------------------- # Object class that handles the robot instructions/syntax class RobotPost(object): # Set default speed in MM/S SPEED_MMS = 10 # ---------------------------------------------------- def pose_2_str(self, pose, joints=None): """Prints a pose target""" track = "" if joints is not None and len(joints) > 6: # T is for the 7th axis (track), so apply the track value self.trackPos = joints[6] track = " T) %.3f" % self.trackPos [x,y,z,w,p,r,] = pose_2_xyzrpw(pose) return ('X) %.3f Y) %.3f Z) %.3f W) %.3f P) %.3f R) %.3f%s' % (x,y,z,w,p,r, track)) def joints_2_str(self, joints): """Prints a joint target""" str_jnts = '' data = ['X','Y','Z','W','P','R','T'] if (len(joints) > 6): self.trackPos = joints[6] else: joints.append(self.trackPos) for i in range(len(joints[:7])): str_jnts = str_jnts + ('%s) %.3f ' % (data[i], joints[i])) str_jnts = str_jnts[:-1] return str_jnts # other variables ROBOT_POST = '' ROBOT_NAME = '' PROG_FILES = [] PROG = [] LOG = '' nAxes = 6 trackPos = 0 POSE_BASE = eye(4) POSE_TOOL = eye(4) def __init__(self, robotpost=None, robotname=None, robot_axes = 6, **kwargs): self.ROBOT_POST = robotpost self.ROBOT_NAME = robotname self.PROG = [] self.LOG = '' self.nAxes = robot_axes def ProgStart(self, progname): progname = FilterName(progname) self.addline('###BEGINNING OF PROGRAM: %s###' % progname) def ProgFinish(self, progname): self.addline('###END OF PROGRAM###') def ProgSave(self, folder, progname, ask_user=False, show_result=False): progname = FilterName(progname) #Convert the prog from txt array import pickle #progPickle = tuple(self.PROG.splitlines()) progPickle = tuple(self.PROG) #test = pickle.load(open("C:\\Users\\Admin\\Documents\\Arduino2\\ARCS source files\MoveLTest","rb")) #test2 = test[2] #print (test2) #print (progPickle[7]) # No extension ? progname = progname + '.txt' # + self.PROG_EXT if ask_user or not DirExists(folder): filesave = getSaveFile(folder, progname, 'Save program as...') if filesave is not None: filesave = filesave.name else: return else: filesave = folder + '/' + progname with open(filesave, "w") as fid: for line in self.PROG: fid.write(line) fid.write("\n") # pickle.dump(progPickle,fid) print('SAVED: %s\n' % filesave) #---------------------- show result if show_result: if type(show_result) is str: # Open file with provided application import subprocess p = subprocess.Popen([show_result, filesave]) else: # open file with default application import os os.startfile(filesave) if len(self.LOG) > 0: mbox('Program generation LOG:\n\n' + self.LOG) def ProgSendRobot(self, robot_ip, remote_path, ftp_user, ftp_pass): """Send a program to the robot using the provided parameters. This method is executed right after ProgSave if we selected the option "Send Program to Robot". The connection parameters must be provided in the robot connection menu of RoboDK""" # TODO: Send code to the robot and execute through serial # This is triggered automatically at the end of the post when we select "Send program to Robot (Ctrl+F6) return def MoveJ(self, pose, joints, conf_RLF=None): """Add a joint movement""" if (pose is not None): self.addline('Move J [*] ' + self.pose_2_str(self.POSE_BASE*pose,joints) + ' Speed-%.1f Ad 10 As 10 Dd 15 Ds 10 $N' % self.SPEED_MMS) else: self.addline('Move J [*] ' + self.joints_2_str(joints) + ' Speed-%.1f Ad 10 As 10 Dd 15 Ds 10 $N' % self.SPEED_MMS) def MoveL(self, pose, joints, conf_RLF=None): """Add a linear movement""" if pose is None: msg = "Linear movement using joint targets is not supported. Change the target type to cartesian or use a joint movement." self.addlog(msg) self.RunMessage(msg, True) return self.addline('Move L [*] ' + self.pose_2_str(self.POSE_BASE*pose, joints) + ' Speed-%.1f Ad 10 As 10 Dd 15 Ds 10 $F' % self.SPEED_MMS) def MoveC(self, pose1, joints1, pose2, joints2, conf_RLF_1=None, conf_RLF_2=None): """Add a circular movement""" if pose1 is None or pose2 is None: msg = "Circular movement using joint targets is not supported. Change the target type to cartesian or use a joint movement." self.addlog(msg) self.RunMessage(msg, True) return self.addline('Move C [*] ' + self.pose_2_str(self.POSE_BASE*pose1, joints1) + ' ' + self.pose_2_str(self.POSE_BASE*pose2, joints2)) def setFrame(self, pose, frame_id=None, frame_name=None): """Change the robot reference frame""" self.POSE_BASE = pose # Add comment in the code: self.RunMessage('Using reference: ' + frame_name) self.RunMessage('Set Base ' + self.pose_2_str(pose), True) def setTool(self, pose, tool_id=None, tool_name=None): """Change the robot TCP""" self.POSE_TOOL = pose self.RunMessage('Using tool: ' + tool_name) self.RunMessage('Set Tool ' + self.pose_2_str(pose), True) def Pause(self, time_ms): """Pause the robot program""" if time_ms < 0: self.addline('PAUSE') else: self.addline('WAIT %.3f' % (time_ms*0.001)) def setSpeed(self, speed_mms): """Changes the robot speed (in mm/s)""" self.SPEED_MMS = speed_mms def setAcceleration(self, accel_mmss): """Changes the robot acceleration (in mm/s2)""" self.addlog('setAcceleration not defined') def setSpeedJoints(self, speed_degs): """Changes the robot joint speed (in deg/s)""" self.addlog('setSpeedJoints not defined') def setAccelerationJoints(self, accel_degss): """Changes the robot joint acceleration (in deg/s2)""" self.addlog('setAccelerationJoints not defined') def setZoneData(self, zone_mm): """Changes the rounding radius (aka CNT, APO or zone data) to make the movement smoother""" self.RunMessage('Set Rounding %.3f mm' % zone_mm, True) def setDO(self, io_var, io_value): """Sets a variable (digital output) to a given value""" if type(io_var) != str: # set default variable name if io_var is a number io_var = 'OUT[%s]' % str(io_var) if type(io_value) != str: # set default variable value if io_value is a number if io_value > 0: io_value = 'TRUE' else: io_value = 'FALSE' # at this point, io_var and io_value must be string values self.addline('%s=%s' % (io_var, io_value)) def setAO(self, io_var, io_value): """Set an Analog Output""" self.setDO(io_var, io_value) def waitDI(self, io_var, io_value, timeout_ms=-1): """Waits for a variable (digital input) io_var to attain a given value io_value. Optionally, a timeout can be provided.""" if type(io_var) != str: # set default variable name if io_var is a number io_var = 'IN[%s]' % str(io_var) if type(io_value) != str: # set default variable value if io_value is a number if io_value > 0: io_value = 'TRUE' else: io_value = 'FALSE' # at this point, io_var and io_value must be string values if timeout_ms < 0: self.addline('WAIT FOR %s==%s' % (io_var, io_value)) else: self.addline('WAIT FOR %s==%s TIMEOUT=%.1f' % (io_var, io_value, timeout_ms)) def RunCode(self, code, is_function_call = False): """Adds code or a function call""" if is_function_call: code = FilterName(code) if not code.endswith(')'): code = code + '()' self.addline(code) else: self.addline(code) def RunMessage(self, message, iscomment = False): """Display a message in the robot controller screen (teach pendant)""" if iscomment: self.addline('# ' + message) else: self.addline('# Display message: ' + message) # ------------------ private ---------------------- def addline(self, newline): """Add a program line""" #self.PROG = self.PROG + newline + '\n' self.PROG.append(newline) def addlog(self, newline): """Add a log message""" self.LOG = self.LOG + newline + '\n' # ------------------------------------------------- # ------------ For testing purposes --------------- def Pose(xyzrpw): [x,y,z,r,p,w] = xyzrpw a = r*math.pi/180 b = p*math.pi/180 c = w*math.pi/180 ca = math.cos(a) sa = math.sin(a) cb = math.cos(b) sb = math.sin(b) cc = math.cos(c) sc = math.sin(c) return Mat([[cb*ca, ca*sc*sb - cc*sa, sc*sa + cc*ca*sb, x],[cb*sa, cc*ca + sc*sb*sa, cc*sb*sa - ca*sc, y],[-sb, cb*sc, cc*cb, z],[0,0,0,1]]) def test_post(): """Test the post with a basic program""" robot = RobotPost() robot.ProgStart("Program") robot.RunMessage("Program generated by RoboDK using a custom post processor", True) robot.setFrame(Pose([807.766544, -963.699898, 41.478944, 0, 0, 0]), 1, "Reference 1") robot.setTool(Pose([62.5, -108.253175, 100, -60, 90, 0]), 2, "Tool 2") robot.MoveJ(Pose([200, 200, 500, 180, 0, 180]), [-46.18419, -6.77518, -20.54925, 71.38674, 49.58727, -302.54752] ) robot.MoveL(Pose([200, 250, 348.734575, 180, 0, -150]), [-41.62707, -8.89064, -30.01809, 60.62329, 49.66749, -258.98418] ) robot.MoveL(Pose([200, 200, 262.132034, 180, 0, -150]), [-43.73892, -3.91728, -35.77935, 58.57566, 54.11615, -253.81122] ) robot.RunMessage("Setting air valve 1 on") robot.RunCode("TCP_On", True) robot.Pause(1000) robot.MoveL(Pose([200, 250, 348.734575, 180, 0, -150]), [-41.62707, -8.89064, -30.01809, 60.62329, 49.66749, -258.98418] ) robot.MoveL(Pose([250, 300, 278.023897, 180, 0, -150]), [-37.52588, -6.32628, -34.59693, 53.52525, 49.24426, -251.44677] ) robot.MoveL(Pose([250, 250, 191.421356, 180, 0, -150]), [-39.75778, -1.04537, -40.37883, 52.09118, 54.15317, -246.94403] ) robot.RunMessage("Setting air valve off") robot.RunCode("TCP_Off", True) robot.Pause(1000) robot.MoveL(Pose([250, 300, 278.023897, 180, 0, -150]), [-37.52588, -6.32628, -34.59693, 53.52525, 49.24426, -251.44677] ) robot.MoveL(Pose([250, 200, 278.023897, 180, 0, -150]), [-41.85389, -1.95619, -34.89154, 57.43912, 52.34162, -253.73403] ) robot.MoveL(Pose([250, 150, 191.421356, 180, 0, -150]), [-43.82111, 3.29703, -40.29493, 56.02402, 56.61169, -249.23532] ) robot.MoveJ(None, [-46.18419, -6.77518, -20.54925, 71.38674, 49.58727, -302.54752] ) robot.ProgFinish("Program") # robot.ProgSave(".","Program",False,False) for line in robot.PROG: print(line) if len(robot.LOG) > 0: mbox('Program generation LOG:\n\n' + robot.LOG) input("Press Enter to close...") if __name__ == "__main__": """Function to call when the module is executed by itself: test""" test_post() def getRobotPosition(): commandCalc = "GP"+"U"+str(J1StepCur)+"V"+str(J2StepCur)+"W"+str(J3StepCur)+"X"+str(J4StepCur)+"Y"+str(J5StepCur)+"Z"+str(J6StepCur)+"\n" serial.write(commandCalc.encode()) RobotCode = str(serial.readline()) Pcode = RobotCode[2:4] #Interpret robot code here #Code from ARCS def MoveNew(J1out,J2out,J3out,J4out,J5out,J6out,newSpeed,ACCdur,ACCspd,DECdur,DECspd,Track,Code): #global xboxUse #if xboxUse != 1: # almStatusLab.config(text="SYSTEM READY", bg = "cornflowerblue") # almStatusLab2.config(text="SYSTEM READY", bg = "cornflowerblue") global J1AngCur global J2AngCur global J3AngCur global J4AngCur global J5AngCur global J6AngCur global J1StepCur global J2StepCur global J3StepCur global J4StepCur global J5StepCur global J6StepCur global TrackcurPos global TrackLength global TrackStepLim global commandCalc J1newAng = J1out J2newAng = J2out J3newAng = J3out J4newAng = J4out J5newAng = J5out J6newAng = J6out TrackNew = Track ###CHECK WITHIN ANGLE LIMITS #if (J1newAng < J1NegAngLim or J1newAng > J1PosAngLim) or (J2newAng < J2NegAngLim or J2newAng > J2PosAngLim) or (J3newAng < J3NegAngLim or J3newAng > J3PosAngLim) or (J4newAng < J4NegAngLim or J4newAng > J4PosAngLim) or (J5newAng < J5NegAngLim or J5newAng > J5PosAngLim) or (J6newAng < J6NegAngLim or J6newAng > J6PosAngLim or TrackNew < 0 or TrackNew > TrackLength): #almStatusLab.config(text="AXIS LIMIT", bg = "red") #almStatusLab2.config(text="AXIS LIMIT", bg = "red") #tab1.runTrue = 0 if (J1newAng < J1NegAngLim or J1newAng > J1PosAngLim): almStatusLab.config(text="J1 AXIS LIMIT", bg = "red") almStatusLab2.config(text="J1 AXIS LIMIT", bg = "red") Curtime = datetime.datetime.now().strftime("%B %d %Y - %I:%M%p") tab6.ElogView.insert(END, Curtime+" - "+"J1 AXIS LIMIT") value=tab6.ElogView.get(0,END) pickle.dump(value,open("ErrorLog","wb")) tab1.runTrue = 0 elif (J2newAng < J2NegAngLim or J2newAng > J2PosAngLim): almStatusLab.config(text="J2 AXIS LIMIT", bg = "red") almStatusLab2.config(text="J2 AXIS LIMIT", bg = "red") Curtime = datetime.datetime.now().strftime("%B %d %Y - %I:%M%p") tab6.ElogView.insert(END, Curtime+" - "+"J2 AXIS LIMIT") value=tab6.ElogView.get(0,END) pickle.dump(value,open("ErrorLog","wb")) tab1.runTrue = 0 elif (J3newAng < J3NegAngLim or J3newAng > J3PosAngLim): almStatusLab.config(text="J3 AXIS LIMIT", bg = "red") almStatusLab2.config(text="J3 AXIS LIMIT", bg = "red") Curtime = datetime.datetime.now().strftime("%B %d %Y - %I:%M%p") tab6.ElogView.insert(END, Curtime+" - "+"J3 AXIS LIMIT") value=tab6.ElogView.get(0,END) pickle.dump(value,open("ErrorLog","wb")) tab1.runTrue = 0 elif (J4newAng < J4NegAngLim or J4newAng > J4PosAngLim): almStatusLab.config(text="J4 AXIS LIMIT", bg = "red") almStatusLab2.config(text="J4 AXIS LIMIT", bg = "red") Curtime = datetime.datetime.now().strftime("%B %d %Y - %I:%M%p") tab6.ElogView.insert(END, Curtime+" - "+"J4 AXIS LIMIT") value=tab6.ElogView.get(0,END) pickle.dump(value,open("ErrorLog","wb")) tab1.runTrue = 0 elif (J5newAng < J5NegAngLim or J5newAng > J5PosAngLim): almStatusLab.config(text="J5 AXIS LIMIT", bg = "red") almStatusLab2.config(text="J5 AXIS LIMIT", bg = "red") Curtime = datetime.datetime.now().strftime("%B %d %Y - %I:%M%p") tab6.ElogView.insert(END, Curtime+" - "+"J5 AXIS LIMIT") value=tab6.ElogView.get(0,END) pickle.dump(value,open("ErrorLog","wb")) tab1.runTrue = 0 elif (J6newAng < J6NegAngLim or J6newAng > J6PosAngLim): almStatusLab.config(text="J6 AXIS LIMIT", bg = "red") almStatusLab2.config(text="J6 AXIS LIMIT", bg = "red") Curtime = datetime.datetime.now().strftime("%B %d %Y - %I:%M%p") tab6.ElogView.insert(END, Curtime+" - "+"J6 AXIS LIMIT") value=tab6.ElogView.get(0,END) pickle.dump(value,open("ErrorLog","wb")) tab1.runTrue = 0 else: ##J1 calc## if (float(J1newAng) >= float(J1AngCur)): #calc pos dir output if (J1motdir == "0"): J1drivedir = "1" else: J1drivedir = "0" J1dir = J1drivedir J1calcAng = float(J1newAng) - float(J1AngCur) J1steps = int(J1calcAng / J1DegPerStep) if Code != 3: J1StepCur = J1StepCur + J1steps #Invert J1AngCur = round(J1NegAngLim + (J1StepCur * J1DegPerStep),2) J1steps = str(J1steps) elif (float(J1newAng) < float(J1AngCur)): J1dir = J1motdir J1calcAng = float(J1AngCur) - float(J1newAng) J1steps = int(J1calcAng / J1DegPerStep) if Code != 3: J1StepCur = J1StepCur - J1steps #Invert J1AngCur = round(J1NegAngLim + (J1StepCur * J1DegPerStep),2) J1steps = str(J1steps) ##J2 calc## if (float(J2newAng) >= float(J2AngCur)): #calc pos dir output if (J2motdir == "0"): J2drivedir = "1" else: J2drivedir = "0" J2dir = J2drivedir J2calcAng = float(J2newAng) - float(J2AngCur) J2steps = int(J2calcAng / J2DegPerStep) if Code != 3: J2StepCur = J2StepCur + J2steps #Invert J2AngCur = round(J2NegAngLim + (J2StepCur * J2DegPerStep),2) J2steps = str(J2steps) elif (float(J2newAng) < float(J2AngCur)): J2dir = J2motdir J2calcAng = float(J2AngCur) - float(J2newAng) J2steps = int(J2calcAng / J2DegPerStep) if Code != 3: J2StepCur = J2StepCur - J2steps #Invert J2AngCur = round(J2NegAngLim + (J2StepCur * J2DegPerStep),2) J2steps = str(J2steps) ##J3 calc## if (float(J3newAng) >= float(J3AngCur)): #calc pos dir output if (J3motdir == "0"): J3drivedir = "1" else: J3drivedir = "0" J3dir = J3drivedir J3calcAng = float(J3newAng) - float(J3AngCur) J3steps = int(J3calcAng / J3DegPerStep) if Code != 3: J3StepCur = J3StepCur + J3steps #Invert J3AngCur = round(J3NegAngLim + (J3StepCur * J3DegPerStep),2) J3steps = str(J3steps) elif (float(J3newAng) < float(J3AngCur)): J3dir = J3motdir J3calcAng = float(J3AngCur) - float(J3newAng) J3steps = int(J3calcAng / J3DegPerStep) if Code != 3: J3StepCur = J3StepCur - J3steps #Invert J3AngCur = round(J3NegAngLim + (J3StepCur * J3DegPerStep),2) J3steps = str(J3steps) ##J4 calc## if (float(J4newAng) >= float(J4AngCur)): #calc pos dir output if (J4motdir == "0"): J4drivedir = "1" else: J4drivedir = "0" J4dir = J4drivedir J4calcAng = float(J4newAng) - float(J4AngCur) J4steps = int(J4calcAng / J4DegPerStep) if Code != 3: J4StepCur = J4StepCur + J4steps #Invert J4AngCur = round(J4NegAngLim + (J4StepCur * J4DegPerStep),2) J4steps = str(J4steps) elif (float(J4newAng) < float(J4AngCur)): J4dir = J4motdir J4calcAng = float(J4AngCur) - float(J4newAng) J4steps = int(J4calcAng / J4DegPerStep) if Code != 3: J4StepCur = J4StepCur - J4steps #Invert J4AngCur = round(J4NegAngLim + (J4StepCur * J4DegPerStep),2) J4steps = str(J4steps) ##J5 calc## if (float(J5newAng) >= float(J5AngCur)): #calc pos dir output if (J5motdir == "0"): J5drivedir = "1" else: J5drivedir = "0" J5dir = J5drivedir J5calcAng = float(J5newAng) - float(J5AngCur) J5steps = int(J5calcAng / J5DegPerStep) if Code != 3: J5StepCur = J5StepCur + J5steps #Invert J5AngCur = round(J5NegAngLim + (J5StepCur * J5DegPerStep),2) J5steps = str(J5steps) elif (float(J5newAng) < float(J5AngCur)): J5dir = J5motdir J5calcAng = float(J5AngCur) - float(J5newAng) J5steps = int(J5calcAng / J5DegPerStep) if Code != 3: J5StepCur = J5StepCur - J5steps #Invert J5AngCur = round(J5NegAngLim + (J5StepCur * J5DegPerStep),2) J5steps = str(J5steps) ##J6 calc## if (float(J6newAng) >= float(J6AngCur)): #calc pos dir output if (J6motdir == "0"): J6drivedir = "1" else: J6drivedir = "0" J6dir = J6drivedir J6calcAng = float(J6newAng) - float(J6AngCur) J6steps = int(J6calcAng / J6DegPerStep) if Code != 3: J6StepCur = J6StepCur + J6steps #Invert J6AngCur = round(J6NegAngLim + (J6StepCur * J6DegPerStep),2) J6steps = str(J6steps) elif (float(J6newAng) < float(J6AngCur)): J6dir = J6motdir J6calcAng = float(J6AngCur) - float(J6newAng) J6steps = int(J6calcAng / J6DegPerStep) if Code != 3: J6StepCur = J6StepCur - J6steps #Invert J6AngCur = round(J6NegAngLim + (J6StepCur * J6DegPerStep),2) J6steps = str(J6steps) ##Track calc## if (TrackNew >= TrackcurPos): TRdir = "1" TRdist = TrackNew - TrackcurPos TRstep = str(int((TrackStepLim/TrackLength)*TRdist)) else: TRdir = "0" TRdist = TrackcurPos - TrackNew TRstep = str(int((TrackStepLim/TrackLength)*TRdist)) TrackcurPos = TrackNew TrackcurEntryField.delete(0, 'end') TrackcurEntryField.insert(0,str(TrackcurPos)) commandCalc = "MJA"+J1dir+J1steps+"B"+J2dir+J2steps+"C"+J3dir+J3steps+"D"+J4dir+J4steps+"E"+J5dir+J5steps+"F"+J6dir+J6steps+"T"+TRdir+TRstep+"S"+newSpeed+"G"+ACCdur+"H"+ACCspd+"I"+DECdur+"K"+DECspd+"U"+str(J1StepCur)+"V"+str(J2StepCur)+"W"+str(J3StepCur)+"X"+str(J4StepCur)+"Y"+str(J5StepCur)+"Z"+str(J6StepCur)+"\n" if Code == 0: ser.write(commandCalc.encode()) ser.flushInput() time.sleep(.01) #ser.read() RobotCode = str(ser.readline()) Pcode = RobotCode[2:4] if (Pcode == "01"): applyRobotCal(RobotCode) J1curAngEntryField.delete(0, 'end') J1curAngEntryField.insert(0,str(J1AngCur)) J2curAngEntryField.delete(0, 'end') J2curAngEntryField.insert(0,str(J2AngCur)) J3curAngEntryField.delete(0, 'end') J3curAngEntryField.insert(0,str(J3AngCur)) J4curAngEntryField.delete(0, 'end') J4curAngEntryField.insert(0,str(J4AngCur)) J5curAngEntryField.delete(0, 'end') J5curAngEntryField.insert(0,str(J5AngCur)) J6curAngEntryField.delete(0, 'end') J6curAngEntryField.insert(0,str(J6AngCur)) CalcFwdKin() DisplaySteps() savePosData() if Code == 2 or Code == 3 : return(commandCalc)