def foo_suck_3(self):
time.sleep(0.1)
# command = bytes([0xAA, 0xAA, 4, 131, 0x03, self.checksum])
command = Message()
command.len = 2 + 2
command.id = 131
command.params = bytes([0x12, 0x00])
command.header = bytes([0xAA, 0xAA])
command.ctrl = 0x03
print(command.bytes())
# command.checksum = None
self.ser.write(command.bytes())
def _get_pose(self):
msg = Message()
msg.id = 10
msg.len = 0x02
msg.ctrl = 0x00
# print(msg.bytes())
response = self._send_command(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.verbose:
print(
"pydobot: 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