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)
