# Copyright 2015-2021 - 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. def get_safe_name(progname, max_chars = 10): """Get a safe program name""" # Remove special characters for c in r' -[]/\;,><&*:%=+@!#^()|?^': progname = progname.replace(c,'') # Set a program name by default: if len(progname) <= 0: progname = 'Program' # Force the program to start with a letter (not a number) if progname[0].isdigit(): progname = 'P' + progname # Set the maximum size of a program (number of characters) if len(progname) > max_chars: progname = progname[:max_chars] return progname # ---------------------------------------------------- # Import RoboDK tools from robodk.robolink import * from robodk.robomath import * from robodk.robofileio import * from robodk.robodialogs import * import sys import os RDK = Robolink() # ---------------------------------------------------- # Object class that handles the robot instructions/syntax class RobotPost(object): """Robot post object defined for Fanuc robots""" # maximum number of lines per program. It will then generate multiple "pages" (files). # This setting can be overriden by RoboDK settings (Tools-Options-Program) MAX_LINES_X_PROG = 9999 # Generate sub programs INCLUDE_SUB_PROGRAMS = True # set default joint speed (percentage of the total speed) JOINT_SPEED = '20%' # set default cartesian speed motion SPEED = '200mm/sec' # set default CNT value (all motion until smooth value is changed) # CNT_VALUE = 'CNT5' # 5% smoothing (set CNT1-CNT100) CNT_VALUE = 'FINE' # Active UFrame Id (register) ACTIVE_UF = 9 # Active UTool Id (register) ACTIVE_UT = 9 # Spare Position register for calculations (such as setting UFRAME and UTOOL) SPARE_PR = 9 # Custom string to add at the end of each move. For example: Offset,PR[1] CUSTOM_STRING = '' # Set the turntable grup (usually GP2 or GP3) TURNTABLE_GROUP = 'GP2' #TURNTABLE_GROUP = 'GP3' # Always use RoboDK user frames (references) USE_ROBODK_UFRAME = False # Always use RoboDK tool frames (tools) USE_ROBODK_UTOOL = False # Set to True to generate programs compatible with RJ3 controllers (small difference in the program header) FANUC_RJ3_COMPATIBLE = False # Max program characters: MAX_PROG_CHARS = 10 if FANUC_RJ3_COMPATIBLE: MAX_PROG_CHARS = 6 # ideally 8 but we should allow up to 100 programs for program splitting # Compile LS program to TP programs # More help here: https://robodk.com/doc/en/Robots-Fanuc.html#LSvsTP # Set the path to Roboguide WinOLPC tools, alternatively, set to None to prevent generating TP files # This step is ignored if the path does not exist PATH_MAKE_TP = 'C:/Program Files (x86)/FANUC/WinOLPC/bin/' #PATH_MAKE_TP = None # Ignore program compilation # Generate a drip feeder program: this will split long programs in subprograms and load them to the controller as they are executed # Set a file name to use an automatic dripfeeder: Files will be sent over FTP as they are executed # Right click a program and select "Send Program to Robot" to trigger the dripfeeding automatically #DRIPFEED_FILE_NAME = "Fanuc_SendProgram_DripFeed.py" DRIPFEED_FILE_NAME = None # Don't do any dripfeeding # Force user input to save the folder FORCE_POPUP_SAVE = False #--------------------------------------------------------------------------------------------- # Program-specific variables: LINE_COUNT = 0 # Count the number of instructions (limited by MAX_LINES_X_PROG) P_COUNT = 0 # Count the number of P targets in one file nProgs = 0 # Count the number of programs and sub programs LBL_ID_COUNT = 0 # Number of labels used # other variables PROG_EXT = 'LS' # set the program extension ROBOT_POST = '' ROBOT_NAME = '' PROG_FILES = [] # List of Program files to be uploaded through FTP PROG_NAMES = [] # List of PROG NAMES PROG_LIST = [] # List of PROG PROG_NAME = 'unknown' # Original name of the current program (example: ProgA) PROG_NAME_CURRENT = 'unknown' # Auto generated name (different from PROG_NAME if we have more than 1 page per program. Example: ProgA2) nPages = 0 # Count the number of pages PROG_NAMES_MAIN = [] # List of programs called by a main program due to splitting PROG = [] # Save the program lines PROG_TARGETS = [] # Save the program lines (targets section) LOG = '' # Save a log nAxes = 6 # Important: This is usually provided by RoboDK automatically. Otherwise, override the __init__ procedure. AXES_TYPE = ['R','R','R','R','R','R'] # Important: This is usually set up by RoboDK automatically. Otherwise, override the __init__ procedure. # 'R' for rotative axis, 'L' for linear axis, 'T' for external linear axis (linear track), 'J' for external rotative axis (turntable) #AXES_TYPE = ['R','R','R','R','R','R','T','J','J'] #example of a robot with one external linear track axis and a turntable with 2 rotary axes AXES_TRACK = [] AXES_TURNTABLE = [] HAS_TRACK = False HAS_TURNTABLE = False HAS_EXTAXES = False EXTAXES_FLAGS = ['1','*','*','*','*'] # Specific to ARC welding applications SPEED_BACKUP = None LAST_POSE = None ROBOT_IP = 'unknown-ip' # robot IP is provided automatically by RoboDK #Exit label Stack SCOPE_EXIT_LABEL = [] CMOVE_SMOOTH = 0 CMOVE_SMOOTH_VALUE = 0 ZONEDATACUSTOM = 0 DOUTV = ' ' STARTL = False LAST_MOVE = None OUTPUTN = 0 REGSPEED = False PATH_COUNTED = False PATH_LENGTHS = [] def __init__(self, robotpost=None, robotname=None, robot_axes = 6, **kwargs): self.ROBOT_POST = robotpost self.ROBOT_NAME = robotname self.nAxes = robot_axes self.PROG = [] self.LOG = '' #for k,v in kwargs.iteritems(): # python2 for k,v in kwargs.items(): if k == 'lines_x_prog': self.MAX_LINES_X_PROG = v if k == 'axes_type': self.AXES_TYPE = v elif k == 'ip_com': self.ROBOT_IP = v for i in range(len(self.AXES_TYPE)): if self.AXES_TYPE[i] == 'T': self.AXES_TRACK.append(i) self.HAS_TRACK = True self.HAS_EXTAXES = True elif self.AXES_TYPE[i] == 'J': self.AXES_TURNTABLE.append(i) self.HAS_TURNTABLE = True self.HAS_EXTAXES = True def CalculateLengths(self,name): prog = RDK.Item(name,ITEM_TYPE_PROGRAM) inst_count = prog.InstructionCount() instn = 0 distance = 0 first_point = 0 cutting = False while instn < inst_count: ins_nom, ins_type, move_type, isjointtarget, pose, joints = prog.Instruction(instn) if prog.Instruction(instn)[0] == 'doon(70)': if cutting == False: cutting = True first_point = instn+1 else: cutting = False self.PATH_LENGTHS.append(distance) distance = 0 else: if cutting == True: self.addline('Instruction: %s | Target pose: %s' % (prog.Instruction(instn)[0], prog.Instruction(instn)[4])) inst = prog.Instruction(instn)[4] inst2 = 0 if instn+1 < inst_count: inst2 = prog.Instruction(instn+1)[4] if inst2 == None: inst2 = prog.Instruction(first_point)[4] if inst != None and inst2 != None: pos1 = robomath.Pose_2_Fanuc(inst) pos2 = robomath.Pose_2_Fanuc(inst2) distance = distance + robomath.distance(pos1, pos2) instn += 1 for x in self.PATH_LENGTHS: self.addline("Length: %s" %x) def ProgStart(self, progname, new_page = False): progname = get_safe_name(progname, self.MAX_PROG_CHARS) progname_i = progname if new_page: #nPages = len(self.PROG_LIST) if self.nPages == 0: if len(self.PROG_NAMES_MAIN) > 0: print("Can't split %s: Two or more programs are split into smaller programs" % progname) print(self.PROG_NAMES_MAIN) raise Exception("Only one program at a time can be split into smaller programs") self.PROG_NAMES_MAIN.append(self.PROG_NAME) # add the first program in the list to be genrated as a subprogram call self.nPages = self.nPages + 1 self.nPages = self.nPages + 1 progname_i = "%s%i" % (self.PROG_NAME, self.nPages) self.PROG_NAMES_MAIN.append(progname_i) else: if self.nProgs > 1 and not self.INCLUDE_SUB_PROGRAMS: return self.PROG_NAME = progname self.nProgs = self.nProgs + 1 #self.PROG_NAMES = [] self.PROG_NAME_CURRENT = progname_i self.PROG_NAMES.append(progname_i) def ProgFinish(self, progname, new_page = False): progname = get_safe_name(progname, self.MAX_PROG_CHARS) if not new_page: # Reset page count self.nPages = 0 #if self.nPROGS > 1: # # Fanuc does not support defining multiple programs in the same file, so one program per file # return header = '' header = header + ('/PROG %s' % self.PROG_NAME_CURRENT) + '\n' # Use the latest name set at ProgStart header = header + '/ATTR' + '\n' header = header + 'OWNER\t\t= MNEDITOR;' + '\n' header = header + 'COMMENT\t\t= "RoboDK sequence";' + '\n' header = header + 'PROG_SIZE\t= 0;' + '\n' header = header + 'CREATE\t\t= DATE 31-12-14 TIME 12:00:00;' + '\n' header = header + 'MODIFIED\t= DATE 31-12-14 TIME 12:00:00;' + '\n' header = header + ('FILE_NAME\t= %s;' % self.PROG_NAME_CURRENT) + '\n' header = header + 'VERSION\t\t= 0;' + '\n' header = header + ('LINE_COUNT\t= %i;' % (self.LINE_COUNT)) + '\n' header = header + 'MEMORY_SIZE\t= 0;' + '\n' header = header + 'PROTECT\t\t= READ_WRITE;' + '\n' header = header + 'TCD: STACK_SIZE\t= 0,' + '\n' header = header + ' TASK_PRIORITY\t= 50,' + '\n' header = header + ' TIME_SLICE\t= 0,' + '\n' header = header + ' BUSY_LAMP_OFF\t= 0,' + '\n' header = header + ' ABORT_REQUEST\t= 0,' + '\n' header = header + ' PAUSE_REQUEST\t= 0;' + '\n' if self.HAS_EXTAXES: header = header + 'DEFAULT_GROUP\t= ' + ','.join(self.EXTAXES_FLAGS) if not self.FANUC_RJ3_COMPATIBLE: header = header + ',*,*' header = header + ';\n' else: if self.FANUC_RJ3_COMPATIBLE: header = header + 'DEFAULT_GROUP\t= 1,*,*,*,*;' + '\n' # required for older controllers such as RJ3 else: header = header + 'DEFAULT_GROUP\t= 1,*,*,*,*,*,*;' + '\n' header = header + 'CONTROL_CODE\t= 00000000 00000000;' + '\n' if self.HAS_TURNTABLE: header = header + '/APPL' + '\n' header = header + '' + '\n' header = header + 'LINE_TRACK;' + '\n' header = header + 'LINE_TRACK_SCHEDULE_NUMBER : 0;' + '\n' header = header + 'LINE_TRACK_BOUNDARY_NUMBER : 0;' + '\n' header = header + 'CONTINUE_TRACK_AT_PROG_END : FALSE;' + '\n' header = header + '' + '\n' header = header + '/MN' #header = header + '/MN' + '\n' # Important! Last line should not have \n self.addline('DO[72] = OFF;') self.PROG.insert(0, header) self.PROG.append('/POS') self.PROG += self.PROG_TARGETS self.PROG.append('/END') # Save PROG in PROG_LIST self.PROG_LIST.append(self.PROG) self.PROG = [] self.PROG_TARGETS = [] #self.nLines = 0 self.LINE_COUNT = 0 self.P_COUNT = 0 self.LBL_ID_COUNT = 0 def progsave(self, folder, progname, ask_user = False, show_result = False): #print(folder) #if not folder.endswith('/'): # folder = folder + '/' if ask_user or not DirExists(folder): folder = getSaveFolder(folder,'Select a directory to save your program') if folder is None: # The user selected the Cancel button return filesave = folder + '/' + progname + '.' + self.PROG_EXT #filesave = folder + progname #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", encoding="shift_jis", errors="replace") as fid: #fid.write(self.PROG) for line in self.PROG: fid.write(line) fid.write('\n') print('SAVED: %s\n' % filesave) # tell RoboDK the path of the saved file self.PROG_FILES.append(filesave) # Delete TP file filesave_TP = filesave[:-3] + '.TP' if os.path.isfile(filesave_TP): os.remove(filesave_TP) # open file with default application if show_result: if type(show_result) is str: # Open file with provided application import subprocess p = subprocess.Popen([show_result, filesave]) elif type(show_result) is list: import subprocess p = subprocess.Popen(show_result + [filesave]) else: # open file with default application os.startfile(filesave) #if len(self.LOG) > 0: # mbox('Program generation LOG:\n\n' + self.LOG) # -------- attempt to build from LS to TP with MakeTP --------- # set robot first with setrobot.exe (delete robot.ini file) sFileExtention = '.LS' if self.PATH_MAKE_TP is not None and FileExists(self.PATH_MAKE_TP + 'MakeTP.exe'): print("POPUP: Compiling LS file with MakeTP.exe: %s..." % progname) sFileExtention = '.TP' sys.stdout.flush() import subprocess command = [self.PATH_MAKE_TP + 'MakeTP.exe', filesave.replace('/','\\'), filesave_TP.replace('/','\\'), '/config', self.PATH_MAKE_TP + 'robot.ini'] #output = subprocess.check_output(command) #self.LOG = output.decode('utf-8') self.LOG += 'Program generation for: ' + progname + '\n' with subprocess.Popen(command, stdout=subprocess.PIPE, bufsize=1, universal_newlines=True) as p: for line in p.stdout: line_ok = line.strip() self.LOG += line_ok + '\n' print("POPUP: " + line_ok) sys.stdout.flush() self.LOG += '\n' #Function to generate def ProgGenerateAssistScript(self, folder, prognames, scriptName, progMain, ftpparam, trigger_start=True): filenameToOpen = folder + '/' + scriptName sFileExtention = '.LS' if self.PATH_MAKE_TP is not None and FileExists(self.PATH_MAKE_TP + 'MakeTP.exe'): sFileExtention = '.TP' localProgNamesCopy = [] for progName in prognames: progName = progName + sFileExtention localProgNamesCopy.append(progName) param_list = ftpparam param_list.append(progMain + sFileExtention) param_list.append(str(localProgNamesCopy)) with open(filenameToOpen,"w+") as fid: fid.write(DRIPFEED_FILE % tuple(param_list)) if trigger_start: import subprocess import sys print("POPUP: Running dripfeeder") sys.stdout.flush() #subprocess.call([sys.executable, filenameToOpen], shell = False) command = 'start "" "' + sys.executable + '" "' + filenameToOpen + '"' print("Running command: " + command) sys.stdout.flush() os.system(command) def ProgSave(self, folder, progname, ask_user = False, show_result = False): ask_user = ask_user or self.FORCE_POPUP_SAVE progname = get_safe_name(progname, self.MAX_PROG_CHARS) nfiles = len(self.PROG_LIST) if nfiles >= 1: if self.LINE_COUNT > 0: # Progfinish was not called! print("Warning: ProgFinish was not called properly") self.PROG_LIST.append(self.PROG) self.PROG_NAMES.append("Unknown") self.PROG = [] self.LINE_COUNT = 0 if len(self.PROG_NAMES_MAIN) > 1: # Warning: the program might be cut to a maximum number of chars progname_main = "M_" + self.PROG_NAMES_MAIN[0] self.INCLUDE_SUB_PROGRAMS = True # Force generation of main program self.ProgStart(progname_main) for prog_call in self.PROG_NAMES_MAIN: self.RunCode(prog_call, True) self.ProgFinish(progname_main) # Save the last program added to the PROG_LIST self.PROG = self.PROG_LIST.pop() progname_last = self.PROG_NAMES.pop() self.progsave(folder, progname_last, ask_user, show_result) #------------------------- #self.LOG = '' if len(self.PROG_FILES) == 0: # cancelled by user return first_file = self.PROG_FILES[0] folder_user = getFileDir(first_file) # progname_user = getFileName(self.FILE_SAVED) # Generate each program for i in range(len(self.PROG_LIST)): if show_result and i > 2: show_result = False self.PROG = self.PROG_LIST[i] self.progsave(folder_user, self.PROG_NAMES[i], False, show_result) #Generate Assist program #self.ProgGenerateAssistScript(folder_user,self.PROG_NAMES,"DripFeed.py", self.PROG_NAME) elif nfiles == 1: self.PROG = self.PROG_NAMES[0] self.progsave(folder, progname, ask_user, show_result) if len(self.PROG_FILES[0]): # user selected cancel return first_file = self.PROG_FILES[0] folder_user = getFileDir(first_file) else: print("Warning! Program has not been properly finished") self.progsave(folder, progname, ask_user, show_result) # Important: Generate dripfeed file, however, FTP credentials are not known at this point if self.DRIPFEED_FILE_NAME is not None: self.ProgGenerateAssistScript(folder_user,self.PROG_NAMES, self.DRIPFEED_FILE_NAME, self.PROG_NAME, [self.ROBOT_IP, 'md:/', 'anonymous', ''], False) if show_result and 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""" if self.DRIPFEED_FILE_NAME is None: UploadFTP(self.PROG_FILES, robot_ip, remote_path, ftp_user, ftp_pass) else: first_file = self.PROG_FILES[0] folder_user = getFileDir(first_file) self.ProgGenerateAssistScript(folder_user, self.PROG_NAMES, self.DRIPFEED_FILE_NAME, self.PROG_NAME, [robot_ip, remote_path, ftp_user, ftp_pass]) def MoveJ(self, pose, joints, conf_RLF=None): """Add a joint movement""" self.page_size_control() # Important to control the maximum lines per program and not save last target on new program #------------------------------------------------------- #------------------------------------------------------- if self.STARTL and pose is not None: target_id = self.add_target_cartesian(pose, joints, conf_RLF) self.addline('DO[72] = ON;') move_ins = 'P[%i] %s FINE ;' % (target_id, self.SPEED) self.addline(move_ins, 'L') self.LAST_POSE = pose self.STARTL = False return if self.STARTL and pose is None: target_id = self.add_target_joints(pose, joints) self.addline('DO[72] = ON;') move_ins = 'P[%i] %s FINE ;' % (target_id, self.SPEED) self.addline(move_ins, 'L') self.LAST_POSE = pose self.STARTL = False else: target_id = self.add_target_joints(pose, joints) move_ins = 'P[%i] %s %s %s ;' % (target_id, self.JOINT_SPEED, self.CNT_VALUE,self.CUSTOM_STRING) self.addline(move_ins, 'J') self.LAST_POSE = pose #------------------------------------------------------- #------------------------------------------------------- self.LAST_MOVE = "J" def MoveL(self, pose, joints, conf_RLF=None): """Add a linear movement""" # important to have this check if we don't to move to the same instruction twice if self.LAST_POSE is not None and pose is not None: # Skip adding a new movement if the new position is the same as the last one if distance(pose.Pos(), self.LAST_POSE.Pos()) < 0.001 and pose_angle_between(pose, self.LAST_POSE) < 0.001: return self.page_size_control() # Important to control the maximum lines per program and not save last target on new program if pose is None: target_id = self.add_target_joints(pose, joints) move_ins = 'P[%i] %s %s %s ;' % (target_id, self.SPEED, self.CNT_VALUE,self.CUSTOM_STRING) if self.OUTPUTN == 1: move_ins = 'P[%i] %s FINE ACC1 ;' % (target_id, self.SPEED) if self.REGSPEED == True: move_ins = 'P[%i] R[1]mm/sec FINE ACC1;' %target_id else: target_id = self.add_target_cartesian(pose, joints, conf_RLF) move_ins = 'P[%i] %s %s %s ;' % (target_id, self.SPEED, self.CNT_VALUE,self.CUSTOM_STRING) if self.OUTPUTN == 1: move_ins = 'P[%i] %s FINE ACC1 ;' % (target_id, self.SPEED) if self.REGSPEED == True: move_ins = 'P[%i] R[1]mm/sec FINE ACC1;' %target_id elif self.OUTPUTN == 0: move_ins = 'P[%i] %s FINE ACC1 ;' % (target_id, self.SPEED) if self.REGSPEED == True: move_ins = 'P[%i] R[2]mm/sec FINE ACC1;' %target_id self.addline(move_ins, 'L') self.LAST_POSE = pose self.LAST_MOVE = "L" def MoveC(self, pose1, joints1, pose2, joints2, conf_RLF_1=None, conf_RLF_2=None): """Add a circular movement""" self.page_size_control() # Important to control the maximum lines per program and not save last target on new program target_id1 = self.add_target_cartesian(pose1, joints1, conf_RLF_1) target_id2 = self.add_target_cartesian(pose2, joints2, conf_RLF_2) if self.OUTPUTN == 1: move_ins = 'P[%i] \n P[%i] %s FINE ACC1 ;' % (target_id1, target_id2, self.SPEED) if self.REGSPEED == True: move_ins = 'P[%i] \n P[%i] R[1]mm/sec FINE ACC1;' % (target_id1, target_id2) self.addline(move_ins, 'C') self.LAST_POSE = pose2 else: move_ins = 'P[%i] \n P[%i] %s %s %s ;' % (target_id1, target_id2, self.SPEED, self.CNT_VALUE, self.CUSTOM_STRING) if self.REGSPEED == True: move_ins = 'P[%i] \n P[%i] R[2]mm/sec FINE ACC1;' % (target_id1, target_id2) self.addline(move_ins, 'C') self.LAST_POSE = pose2 #------------------------------------------------------- #------------------------------------------------------- def setFrame(self, pose, frame_id=None, frame_name=None): """Change the robot reference frame""" xyzwpr = Pose_2_Fanuc(pose) if self.USE_ROBODK_UFRAME or frame_id is None or frame_id < 0 or frame_id > 9: for i in range(6): self.addline('PR[%i,%i]=%.3f ;' % (self.SPARE_PR, i+1, xyzwpr[i])) for i in range(6,self.nAxes): self.addline('PR[%i,%i]=%.3f ;' % (self.SPARE_PR, i+1, 0)) self.addline('UFRAME[%i]=PR[%i] ;' % (self.ACTIVE_UF, self.SPARE_PR)) self.addline('UFRAME_NUM=%i ;' % (self.ACTIVE_UF)) else: self.ACTIVE_UF = frame_id self.addline('UFRAME_NUM=%i ;' % (self.ACTIVE_UF)) self.RunMessage('UF%i:%.1f,%.1f,%.1f,%.1f,%.1f,%.1f' % (frame_id, xyzwpr[0], xyzwpr[1], xyzwpr[2], xyzwpr[3], xyzwpr[4], xyzwpr[5]), True) frame_name_lower = frame_name.lower() # Auto detect motion group for post processor if self.HAS_EXTAXES: if 'gp3' in frame_name_lower: self.TURNTABLE_GROUP = 'GP3' self.EXTAXES_FLAGS[2] = '1' elif 'gp2' in frame_name_lower: self.TURNTABLE_GROUP = 'GP2' self.EXTAXES_FLAGS[1] = '1' else: self.EXTAXES_FLAGS[1] = '1' def setTool(self, pose, tool_id=None, tool_name=None): """Change the robot TCP""" xyzwpr = Pose_2_Fanuc(pose) if self.USE_ROBODK_UTOOL or tool_id is None or tool_id < 0 or tool_id > 9: for i in range(6): self.addline('PR[%i,%i]=%.3f ;' % (self.SPARE_PR, i+1, xyzwpr[i])) for i in range(6,self.nAxes): self.addline('PR[%i,%i]=%.3f ;' % (self.SPARE_PR, i+1, 0)) self.addline('UTOOL[%i]=PR[%i] ;' % (self.ACTIVE_UT, self.SPARE_PR)) self.addline('UTOOL_NUM=%i ;' % (self.ACTIVE_UT)) #self.addline('DO[72] = ON;') else: self.ACTIVE_UT = tool_id self.addline('UTOOL_NUM=%i ;' % (self.ACTIVE_UT)) self.RunMessage('UT%i:%.1f,%.1f,%.1f,%.1f,%.1f,%.1f' % (tool_id, xyzwpr[0], xyzwpr[1], xyzwpr[2], xyzwpr[3], xyzwpr[4], xyzwpr[5]), True) def Pause(self, time_ms): """Pause the robot program""" if time_ms <= 0: self.addline('PAUSE ;') else: self.addline('WAIT %.2f(sec) ;' % (time_ms*0.001)) def setSpeed(self, speed_mms): """Changes the robot speed (in mm/s)""" if self.SPEED_BACKUP is None: # Set the normal speed self.SPEED = '%.0fmm/sec' % max(speed_mms, 0.01) # assume 5000 mm/s as 100% self.JOINT_SPEED = '%.0f%%' % max(min(100.0*speed_mms/5000.0, 100.0), 1) # Saturate percentage speed between 1 and 100 else: # Do not alter the speed as we are in ARC movement mode # skip speed settings if it has been overriden self.SPEED_BACKUP = '%.0fmm/sec' % max(speed_mms, 0.01) # assume 5000 mm/s as 100% #self.JOINT_SPEED = '%.0f%%' % max(min(100.0*speed_mms/5000.0, 100.0), 1) # Saturate percentage speed between 1 and 100 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') self.JOINT_SPEED = '%.0f%%' % max(min(100.0*speed_degs/200.0, 100.0), 1) # Saturate percentage speed between 1 and 100 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 zone data approach (makes the movement more smooth)""" self.CNT_VALUE = 'FINE' def setDO(self, io_var, io_value): """Set a Digital Output""" # AR[1-x] = if type(io_var) != str: # set default variable name if io_var is a number io_var = 'DO[%s]' % str(min(32766,max(1,io_var))) if type(io_value) != str: # set default variable value if io_value is a number if io_value > 0: io_value = 'ON' else: io_value = 'OFF' else: #io_value = "("+io_value+")" io_value = io_value # 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 a Analog Output""" if type(io_var) != str: # set default variable name if io_var is a number io_var = 'AO[%s]' % str(min(32766,max(1,io_var))) if type(io_value) != str: # set default variable value if io_value is a number io_value = str(io_value) # We need brackets for expressions! io_value = "("+io_value+")" # at this point, io_var and io_value must be string values self.addline('%s=%s ;' % (io_var, io_value)) def waitDI(self, io_var, io_value, timeout_ms=-1): """Waits for an 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 = 'DI[%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 = 'ON' else: io_value = 'OFF' else: io_value = "("+io_value+")" # at this point, io_var and io_value must be string values if timeout_ms < 0: self.addline('WAIT %s=%s ;' % (io_var, io_value)) else: self.LBL_ID_COUNT = self.LBL_ID_COUNT + 1 self.addline('$WAITTMOUT=%i ;' % round(timeout_ms)) self.addline('WAIT %s=%s TIMEOUT, LBL[%i] ;' % (io_var, io_value, self.LBL_ID_COUNT)) self.addline('MESSAGE[Timed out for LBL %i] ;' % self.LBL_ID_COUNT) self.addline('PAUSE ;') self.addline('LBL[%i] ;' % self.LBL_ID_COUNT) def addlastline(self, add_params): """Add parameters to the last command""" if len(self.PROG) > 0 and self.PROG[-1].endswith(';'): self.PROG[-1] = self.PROG[-1][:-1] + add_params + ';' # remove last 2 characters def RunCode(self, code, is_function_call = False): """Adds code or a function call""" if is_function_call: # Change the last line to force a Fine movement if len(self.PROG) > 0: str_search = ' ' + self.CNT_VALUE + ' ' str_replace = ' FINE ' self.PROG[-1] = self.PROG[-1].replace(str_search, str_replace) """if len(self.PROG) > 0 and self.ZONEDATACUSTOM: str_search = ' ' + self.CNT_VALUE + ' ' str_replace = ' ' + self.CNT_VALUE self.PROG[-1] = self.PROG[-1].replace(str_search, str_replace) """ if code.lower().startswith("arcstart"): if not code.endswith(')'): code = code + ')' self.ARC_PARAMS = code[9:-1] if len(self.ARC_PARAMS) < 1: # Use default sine wave parameters self.ARC_PARAMS = '2.0Hz,8.0mm,0.075s,0.075' # To impose a desired weld speed, uncomment the following 2 lines: #self.SPEED_BACKUP = self.SPEED #self.SPEED = 'R[20]' # Force a specific weld speed # Provoke ARC START: self.addlastline('Arc Start[11]') # Tune weave with parameters self.addline('Weave Sine[%s] ;' % self.ARC_PARAMS) return # Do not generate default program call elif code.lower().startswith("arcend"): # Provoke ARC END: self.addlastline('Arc End[11]') # Revert to desired speed if self.SPEED_BACKUP is not None: self.SPEED = self.SPEED_BACKUP self.SPEED_BACKUP = None self.ARC_PARAMS = None return # Do not generate default program call #------------------------------------------------------- #------------------------------------------------------- elif code.lower().startswith("doon"): #dout = code[5:-1] self.DOUTV = 'ON' #io_value = self.DOUTV if self.OUTPUTN == 0: self.addline('DO[76] = ON;') self.addline('WAIT 1.0(sec) ;') self.addline('DO[70] = ON;') self.OUTPUTN = 1 elif self.OUTPUTN == 1: self.addline('DO[70] = OFF;') self.addline('DO[76] = OFF;') self.OUTPUTN = 0 '''self.addline('DO[%s]=%s;' %(dout, io_value))''' '''if dout == '76': self.addline('wait 1.50(sec) ;')''' return ''' elif code.lower().startswith("dooff"): dout = code[5:-1] self.DOUTV = 'OFF' io_value = self.DOUTV self.addline('DO[%s]=%s;' %(dout, io_value)) return''' elif code.lower().startswith("reg"): self.REGSPEED = True self.addline('R[1]= 150') self.addline('R[2]= 10') return #------------------------------------------------------- #------------------------------------------------------- elif code.lower().startswith("koriv"): korivsmooth = code[6:-1] self.addline('!--------------------------KORIVEK CNT%s------------------------------------;' %korivsmooth) self.CMOVE_SMOOTH_VALUE = 'CNT' + korivsmooth return #------------------------------------------------------- #------------------------------------------------------- elif code.lower().startswith("startl"): self.addline('!--------------------------ELSO MOZGAS LMOVE------------------------------------;') self.STARTL = True return elif code.lower().startswith("settoolname"): if self.PATH_COUNTED == False: self.PATH_COUNTED = True self.CalculateLengths('APT1_DELTA_TESZT_1_T1') return #------------------------------------------------------- #------------------------------------------------------- iarg = -1 try: iarg = code.index("(") except: pass if iarg > 0: fcn = code[:iarg] args = code[iarg:] fcn = get_safe_name(fcn, self.MAX_PROG_CHARS) self.addline('CALL %s ;' % (fcn+args)) #args = args[1:] #if args.endswith(")"): # args = args[:-1] #arid = 0 #for ar in args.split(","): # arid += 1 # self.addline('AR[%i] = (%s) ;' % (arid, ar.strip())) #self.addline('CALL %s ;' % (fcn)) else: code = get_safe_name(code, self.MAX_PROG_CHARS) self.addline('CALL %s ;' % (code)) else: if not code.endswith(';'): code = code + ';' self.addline(code) def RunMessage(self, message, iscomment = False): """Add a comment or a popup message""" return if iscomment: #pass for i in range(0,len(message), 20): i2 = min(i + 20, len(message)) self.addline('! %s ;' % message[i:i2]) else: for i in range(0,len(message), 20): i2 = min(i + 20, len(message)) self.addline('MESSAGE[%s] ;' % message[i:i2]) #Added with robodk 6.X def EnterIf(self,conditionPair): self.LBL_ID_COUNT = self.LBL_ID_COUNT + 1 uniqueLabelNum = self.LBL_ID_COUNT self.SCOPE_EXIT_LABEL.append(uniqueLabelNum) lhs = conditionPair[0] rhs = conditionPair[1] if type(lhs) != str: # set default variable name if io_var is a number lhs = 'DI[%s]' % str(lhs) if type(rhs) != str: # set default variable value if io_value is a number if rhs > 0: rhs = 'ON' else: rhs = 'OFF' else: rhs = "("+rhs+")" def ExitIf(self): exitLabel = self.SCOPE_EXIT_LABEL.pop() self.addline('LBL[%i] ;' % exitLabel) def EnterWhile(self,conditionPair): self.LBL_ID_COUNT = self.LBL_ID_COUNT + 1 whileStartLabel = self.LBL_ID_COUNT self.LBL_ID_COUNT = self.LBL_ID_COUNT + 1 whileExitLabel = self.LBL_ID_COUNT lhs = conditionPair[0] rhs = conditionPair[1] if type(lhs) != str: # set default variable name if io_var is a number lhs = 'DI[%s]' % str(lhs) if type(rhs) != str: # set default variable value if io_value is a number if rhs > 0: rhs = 'ON' else: rhs = 'OFF' else: rhs = "("+rhs+")" #Push in oposite order they will be retrieved when exiting self.SCOPE_EXIT_LABEL.append(whileExitLabel) self.SCOPE_EXIT_LABEL.append(whileStartLabel) self.addline('LBL[%i] ;' % whileStartLabel) self.addline('IF %s<>%s,JMP LBL[%i] ;' % (lhs,rhs,whileExitLabel)) def ExitWhile(self): whileStartLabel = self.SCOPE_EXIT_LABEL.pop() whileExitLabel = self.SCOPE_EXIT_LABEL.pop() self.addline('JMP LBL[%i] ;' % whileStartLabel) self.addline('LBL[%i] ;' % whileExitLabel) # ------------------ private ---------------------- def page_size_control(self): if self.LINE_COUNT >= self.MAX_LINES_X_PROG: #self.nLines = 0 self.ProgFinish(self.PROG_NAME, True) self.ProgStart(self.PROG_NAME, True) def addline(self, newline, movetype = ' '): """Add a program line""" if self.nProgs > 1 and not self.INCLUDE_SUB_PROGRAMS: return self.page_size_control() self.LINE_COUNT = self.LINE_COUNT + 1 newline_ok = ('%4i:%s ' % (self.LINE_COUNT, movetype)) + newline self.PROG.append(newline_ok) def addline_targets(self, newline): """Add a line at the end of the program (used for targets)""" self.PROG_TARGETS.append(newline) def addlog(self, newline): """Add a log message""" if self.nProgs > 1 and not self.INCLUDE_SUB_PROGRAMS: return self.LOG = self.LOG + newline + '\n' # ------------------ targets ---------------------- def add_target_joints(self, pose, joints): if self.nProgs > 1 and not self.INCLUDE_SUB_PROGRAMS: return self.P_COUNT = self.P_COUNT + 1 add_comma = "" if self.HAS_TRACK: add_comma = "," self.addline_targets('P[%i]{' % self.P_COUNT) self.addline_targets(' GP1:') self.addline_targets(' UF : %i, UT : %i, ' % (self.ACTIVE_UF, self.ACTIVE_UT)) self.addline_targets('\tJ1= %.3f deg,\tJ2= %.3f deg,\tJ3= %.3f deg,' % (joints[0], joints[1], joints[2])) self.addline_targets('\tJ4= %.3f deg,\tJ5= %.3f deg,\tJ6= %.3f deg%s' % (joints[3], joints[4], joints[5], add_comma)) if self.HAS_TRACK: # adding external axes (linear track): track_str = '' for i in range(len(self.AXES_TRACK)): track_str = track_str + '\tE%i=%10.3f mm,' % (i+1, joints[self.AXES_TRACK[i]]) track_str = track_str[:-1] self.addline_targets(track_str) if self.HAS_TURNTABLE: # adding rotative axes (turntable): self.addline_targets(' %s:' % self.TURNTABLE_GROUP) self.addline_targets(' UF : %i, UT : %i,' % (self.ACTIVE_UF, self.ACTIVE_UT)) turntable_str = '' for i in range(len(self.AXES_TURNTABLE)): turntable_str = turntable_str + '\tJ%i=%10.3f deg,' % (i+1, joints[self.AXES_TURNTABLE[i]]) turntable_str = turntable_str[:-1] self.addline_targets(turntable_str) self.addline_targets('};') return self.P_COUNT def add_target_cartesian(self, pose, joints, conf_RLF=None): if self.nProgs > 1 and not self.INCLUDE_SUB_PROGRAMS: return def angle_2_turn(angle): if angle >= 0.0: return +math.floor((+angle+180.0)/360.0) else: return -math.floor((-angle+180.0)/360.0) #return add_target_joints(pose, joints) # using joints as targets is safer to avoid problems setting up the reference frame and configurations xyzwpr = Pose_2_Fanuc(pose) config = ['N','U','T'] #normal #config= ['F','D','B'] #alternative if conf_RLF is not None: if conf_RLF[2] > 0: config[0] = 'F' # F means axis 5 >= 0, N means axis 5 < 0 if conf_RLF[1] > 0: config[1] = 'D' if conf_RLF[0] > 0: config[2] = 'B' turnJ1 = angle_2_turn(joints[0]) turnJ4 = angle_2_turn(joints[3]) turnJ6 = angle_2_turn(joints[5]) self.P_COUNT = self.P_COUNT + 1 add_comma = "" if self.HAS_TRACK: add_comma = "," self.addline_targets('P[%i]{' % self.P_COUNT) self.addline_targets(' GP1:') self.addline_targets(' UF : %i, UT : %i, CONFIG : \'%c %c %c, %i, %i, %i\',' % (self.ACTIVE_UF, self.ACTIVE_UT, config[0], config[1], config[2], turnJ1, turnJ4, turnJ6)) self.addline_targets('\tX =%10.3f mm,\tY =%10.3f mm,\tZ =%10.3f mm,' % (xyzwpr[0], xyzwpr[1], xyzwpr[2])) self.addline_targets('\tW =%10.3f deg,\tP =%10.3f deg,\tR =%10.3f deg%s' % (xyzwpr[3], xyzwpr[4], xyzwpr[5], add_comma)) if self.HAS_TRACK: # adding external axes (linear track): track_str = '' for i in range(len(self.AXES_TRACK)): track_str = track_str + '\tE%i=%10.3f mm,' % (i+1, joints[self.AXES_TRACK[i]]) track_str = track_str[:-1] self.addline_targets(track_str) if self.HAS_TURNTABLE: # adding rotative axes (turntable): self.addline_targets(' %s:' % self.TURNTABLE_GROUP) self.addline_targets(' UF : %i, UT : %i,' % (self.ACTIVE_UF, self.ACTIVE_UT)) turntable_str = '' for i in range(len(self.AXES_TURNTABLE)): turntable_str = turntable_str + '\tJ%i=%10.3f deg,' % (i+1, joints[self.AXES_TURNTABLE[i]]) if i == 2: turntable_str += '\n' turntable_str = turntable_str[:-1] self.addline_targets(turntable_str) self.addline_targets('};') return self.P_COUNT if __name__ == "__main__": """Function to call when the module is executed by itself: test""" test_post2()