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 _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 _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 _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 _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 _setQueuedCmdClear(self):
msg = Message()
msg.id = 245
msg.ctrl = 0x01
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)