def getPoseL(self):
msg = Message()
msg.id = 13
res = self._sendCMD(msg)
poseL = struct.unpack_from('f', res.params, 0)[0]
print(TAG, ': <<<< poseL: ', poseL)
return poseL
def _getPoseL(self):
msg = Message()
msg.id = 13
res = self._sendCMD(msg)
poseL = struct.unpack_from('f', res.params, 0)[0]
print(TAG, '-PoseL: ', poseL)
x = self._get_device_L()
return poseL
def _setPTPCommonParams(self, velocity, acceleration):
msg = Message()
msg.id = 83
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', velocity)))
msg.params.extend(bytearray(struct.pack('f', acceleration)))
return self._sendCMD(msg)
def _getQueuedCmdCurrentIndex(self):
msg = Message()
msg.id = 246
response = self._sendCMD(msg)
if response and response.id == 246:
return self._extractCMDIndex(response)
else:
return -1
def _setPTPJumpParams(self, jump, limit):
msg = Message()
msg.id = 82
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', jump)))
msg.params.extend(bytearray(struct.pack('f', limit)))
return self._sendCMD(msg)
def _setPTPLParams(self, v, a):
msg = Message()
msg.id = 85
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', v)))
msg.params.extend(bytearray(struct.pack('f', a)))
return self._sendCMD(msg)
def _getPTPLParams(self):
msg = Message()
msg.id = 132
response = self._sendCMD(msg)
add = struct.unpack_from('I', response.params, 0)[0]
v = struct.unpack_from('f', response.params, 4)[0]
print(TAG, ' : add--> ', add)
print(TAG, ' : freq--> ', v)
#print(TAG, ' : duty--> ', a)
return response
def _setHomeCoordinate(self, x, y, z, r):
msg = Message()
msg.id = 30
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', x)))
msg.params.extend(bytearray(struct.pack('f', y)))
msg.params.extend(bytearray(struct.pack('f', z)))
msg.params.extend(bytearray(struct.pack('f', r)))
return self._sendCMD(msg)
def _setEndEffectorGripper(self, enable=False):
msg = Message()
msg.id = 63
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray([0x01]))
if enable is True:
msg.params.extend(bytearray([0x01]))
else:
msg.params.extend(bytearray([0x00]))
return self._sendCMD(msg)
def _setModeL(self, enable=True):
msg = Message()
msg.id = 3
msg.ctrl = 0x01
msg.params = bytearray([])
if (enable == True):
msg.params.extend(bytearray([0x01]))
msg.params.extend(bytearray([0x00]))
else:
msg.params.extend(bytearray([0x00]))
return self._sendCMD(msg)
def _getPTPJointParams(self):
msg = Message()
msg.id = 80
j = 0
v, a = []
for i in range(0, 12, 4):
v[j] = struct.unpack_from('f', response.params, i)[0]
j = j + 1
j = 0
for i in range(16, 28, 4):
a[j] = struct.unpack_from('f', response.params, i)[0]
j = j + 1
return v, a
def _setPTPJointParams(self, v, a):
msg = Message()
msg.id = 80
msg.ctrl = 0x03
msg.params = bytearray([])
if (len(v) == 4 and len(a) == 4):
for i in range(4):
msg.params.extend(bytearray(struct.pack('f', v[i])))
for i in range(4):
msg.params.extend(bytearray(struct.pack('f', a[i])))
return self._sendCMD(msg)
else:
print(TAG, ': ERROR: Not enough parameters!')
def _moveTo(self, x, y, z, l, r, mode=MODE):
self._setModeL()
msg = Message()
msg.id = 86
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray([mode]))
msg.params.extend(bytearray(struct.pack('f', x)))
msg.params.extend(bytearray(struct.pack('f', y)))
msg.params.extend(bytearray(struct.pack('f', z)))
msg.params.extend(bytearray(struct.pack('f', r)))
l = 995 if (l > 995) else 20 if (l < 20) else l
msg.params.extend(bytearray(struct.pack('f', l)))
return self._sendCMD(msg)
def _setArcCmd(self, x, y, z, r, cir_x, cir_y, cir_z, cir_r):
msg = Message()
msg.id = 101
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', cir_x)))
msg.params.extend(bytearray(struct.pack('f', cir_y)))
msg.params.extend(bytearray(struct.pack('f', cir_z)))
msg.params.extend(bytearray(struct.pack('f', cir_r)))
msg.params.extend(bytearray(struct.pack('f', x)))
msg.params.extend(bytearray(struct.pack('f', y)))
msg.params.extend(bytearray(struct.pack('f', z)))
msg.params.extend(bytearray(struct.pack('f', r)))
return self._sendCMD(msg)
def _setEndEffectorLaser(self, power=255, enable=False):
"""Enables the laser. Power from 0 to 255. """
msg = Message()
msg.id = 61
msg.ctrl = 0x03
msg.params = bytearray([])
# msg.params.extend(bytearray([0x01]))
if enable is True:
msg.params.extend(bytearray([0x01]))
else:
msg.params.extend(bytearray([0x00]))
# Assuming the last byte is power. Seems to have little effect
msg.params.extend(bytearray([power]))
return self._sendCMD(msg)
def _readMSG(self):
begin_found = False
last_byte = None
tries = 5
while not begin_found and tries > 0:
current_byte = ord(self.ser.read(1))
if current_byte == 170:
if last_byte == 170:
begin_found = True
last_byte = current_byte
tries = tries - 1
if begin_found:
payload_length = ord(self.ser.read(1))
payload_checksum = self.ser.read(payload_length + 1)
if len(payload_checksum) == payload_length + 1:
b = bytearray([0xAA, 0xAA])
b.extend(bytearray([payload_length]))
b.extend(payload_checksum)
msg = Message(b)
if self.con:
print('Lenght', payload_length)
print(payload_checksum)
print('MessageID:', payload_checksum[0])
print(TAG, ': <--',
":".join('{:02x}'.format(x) for x in b))
return msg
return
def _getPose(self):
msg = Message()
msg.id = 10
response = self._sendCMD(msg)
self.x = struct.unpack_from('f', response.params, 0)[0]
self.y = struct.unpack_from('f', response.params, 4)[0]
self.z = struct.unpack_from('f', response.params, 8)[0]
self.r = struct.unpack_from('f', response.params, 12)[0]
self.j1 = struct.unpack_from('f', response.params, 16)[0]
self.j2 = struct.unpack_from('f', response.params, 20)[0]
self.j3 = struct.unpack_from('f', response.params, 24)[0]
self.j4 = struct.unpack_from('f', response.params, 28)[0]
if self.con:
print(
TAG,
': x:%03.1f y:%03.1f z:%03.1f r:%03.1f j1:%03.1f j2:%03.1f j3:%03.1f j4:%03.1f'
% (self.x, self.y, self.z, self.r, self.j1, self.j2, self.j3,
self.j4))
return response
def _setQueuedCmdClear(self):
msg = Message()
msg.id = 245
msg.ctrl = 0x01
return self._sendCMD(msg)
def _getDevSN(self):
msg = Message()
msg.id = 0
return self._sendCMD(msg)
def _setQueuedCmdStartExec(self):
msg = Message()
msg.id = 240
msg.ctrl = 0x01
return self._sendCMD(msg)
def _setHomeCmd(self):
msg = Message()
msg.id = 31
msg.ctrl = 0x03
msg.params = bytearray([])
return self._sendCMD(msg)