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_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 _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_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 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_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 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_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_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_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 _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_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 set_home(self, home: DobotPosition):
message = Message()
message.id = 30
message.ctrl = 0x03
message.params = bytearray([])
message.params.extend(bytearray(struct.pack('f', home.x)))
message.params.extend(bytearray(struct.pack('f', home.y)))
message.params.extend(bytearray(struct.pack('f', home.z)))
message.params.extend(bytearray(struct.pack('f', home.r_head)))
self.dobot._send_message(message)
self.home = home
def get_infrared_sensor(self):
msg = Message()
msg.id = 138
msg.ctrl = 0x00
msg.params = bytearray([])
msg.params.extend(bytearray([0x01])) # port2
msg.params.extend(bytearray([0x01])) # value
response = self._send_command(msg)
# return response
print(response)
return struct.unpack_from('?', response.params, 0)[0]
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_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_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_ptp_cmd(self, x, y, z, r, mode, sync):
msg = Message()
msg.id = SET_PTP_CMD
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)))
response = self._send_command(msg, sync=sync)
return response
def _set_cple_cmd(self, x, y, z, power, absolute=False):
assert 0 <= power <= 100
msg = Message()
msg.id = 92
msg.ctrl = 0x3
msg.params = bytearray([int(absolute)])
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', power)))
return self._send_command(msg)
def io_adc(self, address: int) -> int:
"""
Read the I/O analog-digital conversion value.
:param address: I/O port address(1~20)
:return: I/O analog-digital conversion value
"""
message = Message()
message.id = 134
message.params = bytearray([address])
response = self.dobot._send_command(message, wait=True)
assert False # TODO: Return correct value
def set_io(self, address: int, state: bool):
if not 1 <= address <= 22:
raise DobotException("Invalid address range.")
msg = Message()
msg.id = 131
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('B', address)))
msg.params.extend(bytearray(struct.pack('B', int(state))))
self.wait_for_cmd(self._extract_cmd_index(self._send_command(msg)))
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 _set_ptp_joint_params(self, v_x, v_y, v_z, v_r, a_x, a_y, a_z, a_r):
msg = Message()
msg.id = 80
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', v_x)))
msg.params.extend(bytearray(struct.pack('f', v_y)))
msg.params.extend(bytearray(struct.pack('f', v_z)))
msg.params.extend(bytearray(struct.pack('f', v_r)))
msg.params.extend(bytearray(struct.pack('f', a_x)))
msg.params.extend(bytearray(struct.pack('f', a_y)))
msg.params.extend(bytearray(struct.pack('f', a_z)))
msg.params.extend(bytearray(struct.pack('f', a_r)))
return self._send_command(msg)
def _set_end_effector_laser(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._send_command(msg)
def _set_arc_cmd(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._send_command(msg)
def _set_stepper_motor(self, speed, interface=0, motor_control=True):
msg = Message()
msg.id = 0x87
msg.ctrl = 0x03
msg.params = bytearray([])
if interface == 1:
msg.params.extend(bytearray([0x01]))
else:
msg.params.extend(bytearray([0x00]))
if motor_control is True:
msg.params.extend(bytearray([0x01]))
else:
msg.params.extend(bytearray([0x00]))
msg.params.extend(bytearray(struct.pack('i', speed)))
return self._send_command(msg)
def move_with_conveyor(self,
destination: DobotPosition,
conveyor_position: int,
ptp_mode=default_ptp_mode):
message = Message()
message.id = 86
message.ctrl = 0x03
message.params = bytearray([])
message.params.extend(bytearray([ptp_mode]))
message.params.extend(bytearray(struct.pack('f', destination.x)))
message.params.extend(bytearray(struct.pack('f', destination.y)))
message.params.extend(bytearray(struct.pack('f', destination.z)))
message.params.extend(bytearray(struct.pack('f', destination.r_head)))
message.params.extend(bytearray(struct.pack('f', conveyor_position)))
self.dobot._send_command(message, wait=True)
def _set_jog_coordinate_params(self,
vx,
vy,
vz,
vr,
ax=100,
ay=100,
az=100,
ar=100):
msg = Message()
msg.id = 71
msg.ctrl = 0x03
msg.params = bytearray([])
msg.params.extend(bytearray(struct.pack('f', vx)))
msg.params.extend(bytearray(struct.pack('f', vy)))
msg.params.extend(bytearray(struct.pack('f', vz)))
msg.params.extend(bytearray(struct.pack('f', vr)))
msg.params.extend(bytearray(struct.pack('f', ax)))
msg.params.extend(bytearray(struct.pack('f', ay)))
msg.params.extend(bytearray(struct.pack('f', az)))
msg.params.extend(bytearray(struct.pack('f', ar)))
return self._send_command(msg)
def _set_home_cmd(self):
msg = Message()
msg.id = 31
msg.ctrl = 0x03
msg.params = bytearray([])
return self._send_command(msg)
