def calibrate(self):
message = Message()
message.id = 31
message.ctrl = 0x03
self.dobot.lock.acquire()
self.dobot._send_message(message)
self.dobot.lock.release()
def _set_home_cmd(self, temp):
msg = Message()
msg.id = 31
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('i', temp)))
return self._send_command(msg)
def _set_home_cmd(self):
msg = Message()
msg.id = 31
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.append(0x00)
return self._send_command(msg)
def _setHomeCmd(self):
msg = Message()
msg.id = 31
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('I', 0)))
return self._send_command(msg)
def _set_io_multiplexing(self, address, mode):
msg = Message()
msg.id = 130
msg.ctrl = 0x03
msg.params = bytearray(struct.pack('B', address))
msg.params.extend(bytearray(struct.pack('B', mode)))
return self._send_command(msg)
def _get_queued_cmd_current_index(self):
msg = Message()
msg.id = GET_QUEUED_CMD_CURRENT_INDEX
response = self._send_command(msg)
# sometimes (when a command is running?) the returned struct has more than the index, but the index seems to
# always be the last 8 bytes
return struct.unpack("L", response.params[-8:])[0]
def _set_ptp_l_params(self, velocity, acceleration):
msg = Message()
msg.id = 85
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', velocity)))
msg.params.extend(bytearray(struct.pack('f', acceleration)))
return self._send_command(msg)
def index(self):
msg = Message()
msg.id = 246
msg.ctrl = 0x00
self.i = struct.unpack_from('i', response.params, 0)[0]
return self._send_command(msg)
if self.verbose:
print("index: %i" % (self.i))
def _set_ptp_jump_params(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._send_command(msg)
def _set_jog_cmd(self, isJoint, cmd):
msg = Message()
msg.id = 73
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray([isJoint]))
msg.params.extend(bytearray(struct.pack('i', cmd)))
return self._send_command(msg)
def get_hht_trig_output(self) -> bool:
msg = Message()
msg.id = 41
msg.ctrl = 0
response = self._send_command(msg)
return bool(struct.unpack_from('B', response.params, 0)[0])
def _get_pose_l(self):
msg = Message()
msg.id = 13
response = self._send_command(msg)
self.l = struct.unpack_from('f', response.params, 0)[0]
if self.verbose:
print("pydobot: l:%03.1f" % (self.l))
return response
def _set_jog_command(self, cmd):
msg = Message()
msg.id = 73
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray([0x0]))
msg.params.extend(bytearray([cmd]))
return self._send_command(msg)
def _get_queued_cmd_current_index(self):
msg = Message()
msg.id = 246
response = self._send_command(msg)
if response and response.id == 246:
return self._extract_cmd_index(response)
else:
return -1
def get_color_sensor(self):
msg = Message()
msg.id = 137
msg.ctrl = 0x00
response = self._send_command(msg)
# return response
print(response)
return struct.unpack_from('?', response.params, 0)[0]
def set_color_sensor(self):
msg = Message()
msg.id = 137
msg.ctrl = 0x02
msg.params = bytearray([])
msg.params.extend(bytearray([0x00])) # enable
msg.params.extend(bytearray([0x03])) # port5
return self._send_command(msg)
def set_infrared_sensor(self):
msg = Message()
msg.id = 138
msg.ctrl = 0x02
msg.params = bytearray([])
msg.params.extend(bytearray([0x00])) # enable
msg.params.extend(bytearray([0x01])) # port2
return self._send_command(msg)
def _set_io_pwm(self, address, f, d):
msg = Message()
msg.id = 132
msg.ctrl = 0x03
msg.params = bytearray(struct.pack('B', address))
msg.params.extend(bytearray(struct.pack('f', f)))
msg.params.extend(bytearray(struct.pack('f', d)))
return self._send_command(msg)
def _set_ptp_common_params(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._send_command(msg)
def devicesn(self):
msg = Message()
msg.id = 0
msg.ctrl = 0x00
response = self._send_command(msg)
self.ret = struct.unpack_from('char', response.params)
if self.verbose:
print("pydobot: " % self.ret)
return response
def _set_device_with_l(self, with_l, version):
msg = Message()
msg.id = 3
msg.ctrl = 0x01
msg.params = bytearray([])
msg.params.extend(bytearray([with_l]))
msg.params.extend(bytearray([version]))
msg.params.append(0x00)
return self._send_command(msg)
def set_hht_trig_output(self, state: bool) -> None:
msg = Message()
msg.id = 41
msg.ctrl = 0x02
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('B', int(state))))
self._send_command(msg)
def _set_end_effector_params(self, x, y, z):
msg = Message()
msg.id = SET_END_EFFECTOR_PARAMS
msg.ctrl = 0x02
msg.params = bytearray([])
msg.params.extend(struct.pack('f', x))
msg.params.extend(struct.pack('f', y))
msg.params.extend(struct.pack('f', z))
return self._send_command(msg)
def _set_stepper_motor(self, speed):
msg = Message()
msg.id = 135
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray([0x00]))
msg.params.extend(bytearray([0x01]))
print(speed)
msg.params.extend(bytearray(struct.pack('i', int(speed))))
return self._send_command(msg)
def empty():
msg = Message()
msg.id = 62
msg.ctrl = 0x03
#dist=msg.pose()
msg.params = bytearray([])
msg.params.extend(bytearray([0x00]))
msg.params.extend(bytearray([0x00]))
return device._send_command(msg)
def _set_home_coordinate(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._send_command(msg)
def _set_ptp_coordinate_params(self, velocity, acceleration):
msg = Message()
msg.id = SET_PTP_COORDINATE_PARAMS
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', velocity)))
msg.params.extend(bytearray(struct.pack('f', velocity)))
msg.params.extend(bytearray(struct.pack('f', acceleration)))
msg.params.extend(bytearray(struct.pack('f', acceleration)))
return self._send_command(msg)
def _set_cp_cmd(self, x, y, z):
msg = Message()
msg.id = 91
msg.ctrl = 0x03
msg.params = bytearray(bytes([0x01]))
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.append(0x00)
return self._send_command(msg)
def _set_cp_cmd(self, x, y, z):
msg = Message()
msg.id = 91
msg.ctrl = 0x03
msg.params = bytearray(bytes([0x01]))
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.append(0x00)
return self._send_command(msg)
def _set_ptp_cmd(self, x, y, z, r, mode):
msg = Message()
msg.id = 84
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)))
return self._send_command(msg)
def _set_end_effector_suction_cup(self, suck=False):
msg = Message()
msg.id = 62
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray([0x01]))
if suck is True:
msg.params.extend(bytearray([0x01]))
else:
msg.params.extend(bytearray([0x00]))
return self._send_command(msg)
def _set_ptp_cmd(self, x, y, z, r, mode, wait):
msg = Message()
msg.id = 84
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)))
return self._send_command(msg, wait)
def _set_end_effector_gripper(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._send_command(msg)
def _set_cp_params(self, velocity, acceleration, period):
msg = Message()
msg.id = 90
msg.ctrl = 0x3
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', acceleration)))
msg.params.extend(bytearray(struct.pack('f', velocity)))
msg.params.extend(bytearray(struct.pack('f', period)))
msg.params.extend(bytearray([0x0])) # non real-time mode (what does it mean??)
return self._send_command(msg)
def _get_pose(self):
msg = Message()
msg.id = 10
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
