def sync_read(self, ids, reg, length, port=0, doPrint=False):
groupread_num = dynamixel.groupSyncRead(self.port_num[port], self.protocol, reg, length)
for dxl_id in ids:
# Add parameter storage for Dynamixel#1 present position value
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncReadAddParam(groupread_num, dxl_id)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncRead addparam failed" % (dxl_id))
return False
# Syncread present position
dynamixel.groupSyncReadTxRxPacket(groupread_num)
dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num[port], self.protocol)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(self.protocol, dxl_comm_result))
for dxl_id in ids:
# Check if groupsyncread data of Dynamixels is available
dxl_getdata_result = ctypes.c_ubyte(dynamixel.groupSyncReadIsAvailable(groupread_num, dxl_id, reg, length)).value
if dxl_getdata_result != 1:
print("[ID:%03d] groupSyncRead getdata failed" % (dxl_id))
return False
for dxl_id in ids:
dxl_result = dynamixel.groupSyncReadGetData(groupread_num, dxl_id, reg, length)
print("[ID:%03d] result is %d" % (dxl_id, dxl_result))
return True
def __init__(self, port):
self.port = port
self.protocol = 2
self.resolution = MX_RESOLUTION # MX-28 Resolution
self.groupwrite = dxl.groupSyncWrite(self.port, self.protocol,
ADDR_GOAL_POS, LEN_GOAL_POSITION)
self.groupread = dxl.groupSyncRead(self.port, self.protocol,
ADDR_PRES_POS, LEN_PRESENT_POSITION)
print("Protocol 2 ready..")
def __init__(self, baudrate=1000000, port='/dev/ttyUSB0', protocol=2):
self.baudrate = baudrate
self.port = dxl.portHandler(port.encode('utf-8'))
self.protocol = protocol
self.resolution = MX_RESOLUTION # MX-28 Resolution
self.groupwrite = dxl.groupSyncWrite(self.port, self.protocol,
ADDR_GOAL_POS, LEN_GOAL_POSITION)
self.groupread = dxl.groupSyncRead(self.port, self.protocol,
ADDR_PRES_POS, LEN_PRESENT_POSITION)
dxl.packetHandler()
self.connect()
def __init__(
self,
# Check which port is being used on your controller
device_name="COM5".encode('utf-8'),
# baud rate
baud_rate=1000000,
# motor ids
m1id=100,
m2id=101,
m3id=102,
m4id=103,
#protocol ver
proto_ver=2,
#SyncRead Addr
read_addr=126,
#SyncRead Len
read_len=2):
self.baud_rate = baud_rate
self.device_name = device_name
self.m1id = m1id
self.m2id = m2id
self.m3id = m3id
self.m4id = m4id
self.proto_ver = proto_ver
self.read_addr = read_addr
self.read_len = read_len
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
self.port_num = dynamixel.portHandler(device_name)
dynamixel.setPacketTimeoutMSec(1)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
# Initialize Groupsyncread Structs for Current
self.groupread_num = dynamixel.groupSyncRead(self.port_num, proto_ver,
read_addr,
read_len) #0, 148
dt = datetime.datetime.now()
self.timestamp0 = dt.minute * 60000000 + dt.second * 1000000 + dt.microsecond
self.Init_Port_And_Motors()
self.Init_Param_Storage()
COMM_SUCCESS = 0 # Communication Success result value
COMM_TX_FAIL = -1001 # Communication Tx Failed
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
# Initialize Groupsyncwrite instance
groupwrite_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_XM430_GOAL_POSITION, LEN_XM430_GOAL_POSITION)
# Initialize Groupsyncread Structs for Present Position
groupread_num = dynamixel.groupSyncRead(port_num, PROTOCOL_VERSION, ADDR_XM430_PRESENT_POSITION, LEN_XM430_PRESENT_POSITION)
index = 0
dxl_comm_result = COMM_TX_FAIL # Communication result
dxl_addparam_result = 0 # AddParam result
dxl_getdata_result = 0 # GetParam result
dxl_goal_position = [
[2048, 2048, 3072, 2048, 2048, 2048, 2700], # Init v
[1649, 2028, 1626, 1527, 2683, 2426, 2000], # Bow v
[1853, 2677, 2373, 1731, 2446, 2340, 2000], # Move v
[1853, 2677, 2373, 1731, 2446, 2340, 2560], # Grab
[2048, 2048, 3072, 2048, 2048, 2048, 2560], # Center
[ 1, 2048, 3072, 2048, 2048, 2048, 2560], # Center 2
[ 229, 1122, 3280, 2595, 1793, 1517, 2560], # Move
[ 229, 1122, 3280, 2595, 1793, 1517, 2000], # Loose
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
# Initialize Groupsyncwrite instance
groupwrite_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION,
ADDR_PRO_GOAL_POSITION,
LEN_PRO_GOAL_POSITION)
# Initialize Groupsyncread Structs for Present Position
groupread_num = dynamixel.groupSyncRead(port_num, PROTOCOL_VERSION,
ADDR_PRO_PRESENT_POSITION,
LEN_PRO_PRESENT_POSITION)
index = 0
dxl_comm_result = COMM_TX_FAIL # Communication result
dxl_addparam_result = 0 # AddParam result
dxl_getdata_result = 0 # GetParam result
dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE,
DXL_MAXIMUM_POSITION_VALUE] # Goal position
dxl_error = 0 # Dynamixel error
dxl1_present_position = 0 # Present position
dxl2_present_position = 0
# Open port
if dynamixel.openPort(port_num):
COMM_SUCCESS = 0 # Communication Success result value
COMM_TX_FAIL = -1001 # Communication Tx Failed
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
# Initialize Groupsyncwrite instance
groupwrite_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_PRO_INDIRECTDATA_FOR_WRITE, LEN_PRO_INDIRECTDATA_FOR_WRITE)
# Initialize Groupsyncread Structs for Present Position
groupread_num = dynamixel.groupSyncRead(port_num, PROTOCOL_VERSION, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_INDIRECTDATA_FOR_READ)
index = 0
dxl_comm_result = COMM_TX_FAIL # Communication result
dxl_addparam_result = 0 # AddParam result
dxl_getdata_result = 0 # GetParam result
dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position
dxl_error = 0 # Dynamixel error
dxl_moving = 0 # Dynamixel moving status
dxl_led_value = [0x00, 0xFF] # Dynamixel LED value
dxl_present_position = 0 # Present position
# Open port
if dynamixel.openPort(port_num):
print("Succeeded to open the port!")
# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)
# Initialize PacketHandler Structs
dynamixel.packetHandler()
# Initialize Groupsyncwrite instance
groupwrite_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION,
ADDR_PRO_INDIRECTDATA_FOR_WRITE,
LEN_PRO_INDIRECTDATA_FOR_WRITE)
# Initialize Groupsyncread Structs for Present Position
groupread_num = dynamixel.groupSyncRead(port_num, PROTOCOL_VERSION,
ADDR_PRO_INDIRECTDATA_FOR_READ,
LEN_PRO_INDIRECTDATA_FOR_READ)
index = 0
dxl_comm_result = COMM_TX_FAIL # Communication result
dxl_addparam_result = 0 # AddParam result
dxl_getdata_result = 0 # GetParam result
dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE,
DXL_MAXIMUM_POSITION_VALUE] # Goal position
dxl_error = 0 # Dynamixel error
dxl_moving = 0 # Dynamixel moving status
dxl_led_value = [0x00, 0xFF] # Dynamixel LED value
dxl_present_position = 0 # Present position
# Open port