def changeServoValue(servoname, value): degree = 0 #validate servo exists if (checkKey(paras.servosetting, servoname) == False): return log.getMsg('ERR_SERVO_INVALIDSERVO', servoname + ' does not exists') #if given value is position name, get position degree from servosetting, or direct use integer value valuestr = '' if type(value) is int or type(value) is float: degree = value valuestr = str(valuestr) elif type(value) is str and value != '': # doublecheck, maybe value is number but data type is string if value.strip('-').isnumeric(): degree = float(value) else: degree = paras.servosetting[servoname]['positions'][value] valuestr = value else: return log.getMsg('ERR_SERVO_INVALIDVALUE', 'invalid value') result = hardware.setServo(servoname, degree) if result == "OK": return log.getMsg(result, "") else: maxdeg = paras.servosetting[servoname]['maxdeg'] mindeg = paras.servosetting[servoname]['mindeg'] return log.getMsg( result, "assigned invalid value '" + valuestr + "' to servo '" + servoname + "', please check is it within " + str(mindeg) + "/" + str(maxdeg) + ".")
def api_routine(self): if self.resource is None: list = self.getAllRoutines() separator = "," availableroutine = separator.join(list) return log.getMsg('OK', f"Available routines is {availableroutine}", {'routines': list}) routinename = self.resource othersinfo = self.subresource # direct give routine json content if othersinfo is None: result = self.getRoutineInfo(routinename) if not self.isErrorCode(result): return result else: return log.getMsg( result, f"Invalid json content, please check again {routinename}.json." ) elif othersinfo == "run": result = self.runRoutine(routinename) else: result = "ERR_ROUTINE_OPERATIONUNKNOWN" return log.getMsg(result, "")
def deleteRoutine(routinename): content = getRoutineContent(routinename) if content == "": return log.getMsg("ERR_ROUTE_NOTFOUND", routinename + " not found") else: filename = '../routines' + "/" + routinename + ".json" os.remove(filename) return log.getMsg("OK", routinename + " removed")
def api_calibratetrack(self): if self.resource is None: tracks = self.hw.getAllTracks() separator = ',' txttracks = separator.join(tracks) return log.getMsg("OK", "Available travel track is " + txttracks) else: result = self.calibrateTrack(self.resource) return log.getMsg(result, "")
def api_servo(self): if self.resource is None: servos = self.hw.getAllServos() separator = ',' txtservos = separator.join(servos) return log.getMsg("OK", "Available servo is " + txtservos) else: value = self.req.get("value") result = self.moveServo(self.resource, value) return log.getMsg(result, "")
def api_move_j(self): if self.resource is None: return log.getMsg( "OK", "You can move joint j1-j6, like /move_j/j1?movetype=absolute/move°ree=10" ) else: degree = self.req.get("degree") movetype = self.req.get("movetype") result = self.moveJoint(self.resource, degree, movetype) return log.getMsg(result, "")
def api_movetrack(self): if self.resource is None: tracks = self.hw.getAllTracks() separator = ',' txttracks = separator.join(tracks) return log.getMsg("OK", "Available travel track is " + txttracks) else: mm = self.req.get("mm") movetype = self.req.get("movetype") result = self.moveTrack(self.resource, mm, movetype) return log.getMsg(result, "")
def setServo(self, servoname, degree): if (self.checkKey(self.servosetting, servoname) == False): return log.getMsg('ERR_SERVO_INVALIDSERVO', servoname + ' does not exists') #identify which servo no, cause AR3 put firmware into arduino, recognise as 0,1,2,3 servonumber = 9999 i = 0 for k, v in self.servosetting.items(): if k == servoname: servonumber = i i = i + 1 maxdeg = self.servosetting[servoname]['maxdeg'] mindeg = self.servosetting[servoname]['mindeg'] if degree > maxdeg: return 'ERR_SERVO_MAX' elif degree < mindeg: return 'ERR_SERVO_MIN' degreestr = str(degree) command = "SV" + str(servonumber) + "P" + degreestr board = self.ser_arduino # most of the case, using arduino, this place reserved for future enhancement which add servo into more board result = self.writeIO(board, command) self.servovalue[servoname] = degree self.saveData() return result
def runRoutine(routinename): content = getRoutineContent(routinename) if is_json(content): j = json.loads(content) # for k, v in j.items(): return log.getMsg("OK", "")
def moveServo(self, servoname, value): degree = 0 # validate servo exists if (self.checkKey(paras.servosetting, servoname) == False): return 'ERR_SERVO_INVALIDSERVO' if value is None: return "ERR_SERVO_INVALIDVALUE" # if given value is position name, get position degree from servosetting, or direct use integer value valuestr = '' if type(value) is int or type(value) is float: degree = value valuestr = str(valuestr) elif type(value) is str and value != '': # doublecheck, maybe value is number but data type is string if value.strip('-').isnumeric(): degree = float(value) else: degree = paras.servosetting[servoname]['positions'][value] valuestr = value else: return 'ERR_SERVO_INVALIDVALUE' result = self.hw.setServo(servoname, degree) if result == "OK": return log.getMsg(result, "") else: maxdeg = paras.servosetting[servoname]['maxdeg'] mindeg = paras.servosetting[servoname]['mindeg'] return result
def moveRestPosition(joints): for i in range(0, paras.jointqty): restpos = paras.jsetting[i]["restpos"] jname = 'J' + str(i + 1) if joints[i] == 1: rotateJoint(jname, restpos, 'absolute') return log.getMsg("OK", "all joint at rest position now")
def api_io(self): l = self.getAllIO() if self.resource is None: return log.getMsg( "OK", "Only support digital input/output at the moment", { 'inputpin': l['input'], 'outputpin': l['output'] }) elif self.subresource is None: operation = self.resource.lower() if operation == 'on' or operation == 'off': return log.getMsg( "OK", "You can output digital signal to following pins", {'pins': l['output']}) elif operation == 'read': return log.getMsg( "OK", "You can read digital input from following pins", {'pins': l['input']}) else: return log.getMsg("ERR_IO_INVALIDOPERATION", "") else: operation = self.resource.lower() pinno = int(self.subresource) if operation == 'read': result = self.readDigitalInput(pinno) if self.isErrorCode(result): return log.getMsg(result, "") else: return log.getMsg("OK", "", {"value": result}) #write digital io elif operation == 'on' or operation == 'off': result = self.sendDigitalOutput(pinno, operation) else: result = "ERR_IO_INVALIDOPERATION" return log.getMsg(result, "")
def getRoutineInfo(routinename): content = getRoutineContent(routinename) if content == "": return log.getMsg("ERR_ROUTE_NOTFOUND", routinename + " not found") else: if is_json(content): return json.loads(content) else: return content
def api_calibrate(self): # display calibration options x = self.resource if x == "setrest": result = self.overrideRestPosition() elif x == "all": result = self.calibrateAll() elif len(x) == 2 and self.left(x, 1) == 'j': result = self.calibrateJoints(x) elif x is None: return log.getMsg( 'OK', 'You can calibrate all joint with /calibrate/all, calibrate single joint (j1-j6) like /calibrate/j1, set new rest position /calibrate/setrest' ) else: return log.getMsg('ERR_CALIBRATION_UNKNOWN', x + " is not valid calibration command") return log.getMsg(result, "")
def calibrateTrack(trackname, limitswitch): islimitswitch = 0 if type(limitswitch) is str and limitswitch.isnumeric(): islimitswitch = int(limitswitch) elif type(limitswitch) is int: islimitswitch = limitswitch if islimitswitch == 0: # no limit switch, use current position as 0 result = hardware.setTrackValue(trackname, 0) else: result = hardware.moveTravelTrackToLimitSwitch(trackname) return log.getMsg(result, "Put Travel Track to 0mm")
def runCalibration(jname): global err_log, paras jointname = jname.upper() result = "" if jointname == "SETREST": result = hardware.writeARMPosition('rest', [1, 1, 1, 1, 1, 1]) elif len(jointname) == 2 and left(jointname, 1) == 'J': jointno = int(right(jname, 1)) - 1 result = hardware.calibrateJoint(jointno) log.debug(result) elif jointname == 'ALL': joints = [1, 1, 1, 1, 1, 1] # joints = [1, 0,0,0,0,0] result = hardware.goAllJointLimit(joints) # hardware.moveFromLimitToRestPosition(alljoints) result2 = moveRestPosition(joints) if result != "OK": return log.getMsg(result, "Cannot move all joint into limit switch") else: return log.getMsg("ERR_CALIBRATE_FAILED01", "Invalid calibration joint name " + jointname) # all success result return here return log.getMsg("OK", "")
def getAllPosition(): joinvalues = hardware.refreshStepperMotorEncoderValue() servovalues = hardware.getServoValues() trackvalues = hardware.getTrackValues() txt = '/setposition?' for k, v in joinvalues.items(): txt = txt + 'J' + str(k + 1) + '=' + str(v['degree']) + '&' print("txt=") print(txt) for k, v in servovalues.items(): txt = txt + k + '=' + str(v) + '&' for k, v in trackvalues.items(): txt = txt + k + '=' + str(v['mm']) + '&' return log.getMsg('OK', txt)
def rotateJoint(jname, degree, movetype): log.info("access rotateJoint") #validate movetype is received, and with proper value if type(movetype) is not str: return log.getMsg('ERR_MOVE_INVALIDTYPE', 'move type is not str') else: movetype = movetype.upper() if movetype != 'MOVE' and movetype != 'ABSOLUTE': return log.getMsg('ERR_MOVE_INVALIDTYPE', 'movetype is not move or absolute') if type(degree) is str: degree = float(degree) jointname = jname.upper() if len(jointname) != 2 and left(jointname, 1) != 'J': return log.getMsg('ERR_JOINT_WRONGNAME', 'Joint name shall be J1, J2,j1,j2...') joint_id = int(right(jointname, 1)) - 1 if joint_id < 0 and joint_id > paras.jointqty: return log.getMsg('ERR_JOINT_OUT_OF_RANGE', '') encoders = hardware.refreshStepperMotorEncoderValue() if movetype == 'MOVE': newdegree = encoders[joint_id]['degree'] + degree else: # put joint into absolute degree, need add existing position # newdegree = currentdegree + changedegree newdegree = degree exisdegree = encoders[joint_id]['degree'] degree = newdegree - exisdegree newdegreestr = str(newdegree) result = hardware.rotateJoint(joint_id, degree) maxdegstr = str(paras.jsetting[joint_id]["maxdeg"]) mindegstr = str(paras.jsetting[joint_id]["mindeg"]) if result != 'OK': jsondata = log.getMsg( result, jname + " shift " + str(degree) + " become " + newdegreestr + " which is not within " + mindegstr + "/" + maxdegstr) else: jsondata = log.getMsg( result, jname + " shift " + str(degree) + " become " + newdegreestr + " which is within " + mindegstr + "/" + maxdegstr) log.info("done rotateJoint") return jsondata
def moveJoint(self, jname, degree, movetype): # validate movetype is received, and with proper value if type(movetype) is not str: return 'ERR_MOVE_INVALIDTYPE' else: movetype = movetype.upper() if movetype != 'MOVE' and movetype != 'ABSOLUTE': return 'ERR_MOVE_INVALIDTYPE' if degree is None: return "ERR_JOINT_NODEGREEDEFINED" elif type(degree) is str: degree = float(degree) jointname = jname.lower() if len(jointname) != 2 and self.left(jointname, 1) != 'j': return 'ERR_JOINT_WRONGNAME' joint_id = int(self.right(jointname, 1)) - 1 if joint_id < 0 and joint_id > paras.jointqty: return log.getMsg('ERR_JOINT_OUT_OF_RANGE', '') encoders = self.hw.refreshStepperMotorEncoderValue() if type(encoders) == str: return encoders if movetype == 'MOVE': newdegree = encoders[joint_id]['degree'] + degree else: # put joint into absolute degree, need add existing position # newdegree = currentdegree + changedegree newdegree = degree exisdegree = encoders[joint_id]['degree'] degree = newdegree - exisdegree newdegreestr = str(newdegree) result = self.hw.rotateJoint(joint_id, degree) maxdegstr = str(paras.jsetting[joint_id]["maxdeg"]) mindegstr = str(paras.jsetting[joint_id]["mindeg"]) return result
def moveTrack(trackname, mm, movetype): #at the moment, any trackname also convert to track1 if (checkKey(paras.tracksetting, trackname) == False): return log.getMsg('ERR_TRACK_INVALID', trackname + ' does not exists') # validate movetype is received, and with proper value if type(movetype) is not str: return log.getMsg('ERR_MOVE_INVALIDTYPE', 'movetype is not str') else: movetype = movetype.upper() if movetype != 'MOVE' and movetype != 'ABSOLUTE': return log.getMsg('ERR_MOVE_INVALIDTYPE', 'movetype is not move or absolute') trackdata = hardware.getTrackValues()[trackname] bufferlength = 10 lengthlimit = paras.tracksetting[trackname]['length'] - bufferlength if type(mm) is str: if mm.strip('-').isnumeric(): mm = float(mm) else: return log.getMsg( "ERR_TRACK_WRONG_DATA_TYPE", "'mm' is require numeric value within 0-" + str(lengthlimit)) if movetype == 'MOVE': newmm = trackdata['mm'] + mm else: # put joint into absolute degree, need add existing position # newdegree = currentdegree + changedegree newmm = mm exismm = trackdata['mm'] mm = newmm - exismm newmmstr = str(newmm) if newmm < 0 or newmm > lengthlimit: return log.getMsg( 'ERR_MOVETRACK_OVERLIMIT', "Track " + trackname + " blocked cause you wish to move " + str(mm) + " mm, to " + newmmstr + ". It hit limit 0 - " + str(lengthlimit)) else: result = hardware.moveTrack(trackname, mm) return log.getMsg( result, "Track " + trackname + " moved " + str(mm) + " mm, to " + newmmstr + ', limit 0 - ' + str(lengthlimit))
def addRoutine(): return log.getMsg("OK", "")
def overrideRoutine(): return log.getMsg("OK", "")
def api_movetorestposition(self): result = self.moveToRestPosition([1, 1, 1, 1, 1, 1]) return log.getMsg(result, "")
def api_move_l(self): x = self.req.get('x') y = self.req.get('y') z = self.req.get('z') result = self.moveLinear(x, y, z) return log.getMsg(result, "")
def api_setspeed(self): result = self.setSpeed(self.req.get('percent')) return log.getMsg(result, "")
def run_calibrate(req): return log.getMsg( 'OK', 'You can calibrate all joint with /calibrate/all, calibrate single joint (j1-j6) with /calibrate/j1, or override current arm position as rest position by using /calibrate/setrest' )
def api_setposition(self): allparas = {} for k, v in self.req.items(): allparas[k] = v result = self.setPosition(allparas) return log.getMsg(result, "")
content = getRoutineContent(routinename) if content == "": return log.getMsg("ERR_ROUTE_NOTFOUND", routinename + " not found") else: filename = '../routines' + "/" + routinename + ".json" os.remove(filename) return log.getMsg("OK", routinename + " removed") def addRoutine(): return log.getMsg("OK", "") def overrideRoutine(): return log.getMsg("OK", "") ############## start up routines, initialize data and etc ########################################## try: print("Start") #initSystemVariables() #print("after init system variables") #hardware = Hardware(v, paras) #routine = Routine() # print("hardware type=",type(hardware)) #print("Try connecting and try") #print("Arm connected") except Exception as e: err_log = log.getMsg("ERR_CONNECT_FAILED01", e) print("Failed connect arm ", e)
def api_getpositionurl(self): result = self.getPositionUrl() return log.getMsg("OK", result)