def read_a_block_vals(port, idn, read_block, read_wait_time=0.00001):
""" Reads a block of sensor values from dxl device.
Args:
port: Dynamixel portHandler object
idn: An integer representing the DXL ID number
read_block: An instance of Contiguous Registers (defined in dxl_reg) containing the block of registers to read
read_wait_time: A float representing time (in seconds) to wait before reading the buffer
Returns:
A list containing sensor values of each register in the read_block
"""
dynamixel.readTxRx(port, PROTOCOL_VERSION, idn, read_block.offset,
read_block.width)
dxl_comm_result = dynamixel.getLastTxRxResult(port, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
data_pos = 0
vals = []
for reg in read_block._regs:
data = dynamixel.getDataRead(port, PROTOCOL_VERSION, reg.width,
data_pos)
data_pos += reg.width
vals.append(data)
return read_block.vals_from_data(vals)
def loop_until_written(port, dxl_id, packet, read_wait_time=0.00001):
""" Loop until instruction packet is written in the DXL control table
Args:
port: Dynamixel portHandler object
idn: An integer representing the DXL ID number
packet: A tuple - (address of register, list of values to be written, width of the register in bytes)
read_wait_time: A float representing time (in seconds) to wait before reading the buffer
"""
reg0, buf, width = packet
if width == 1:
write1byte(port, dxl_id, reg0, buf[0])
elif width == 2:
write2bytes(port, dxl_id, reg0, buf[0])
dxl_comm_result = dynamixel.getLastTxRxResult(port, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(port, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))