def set_target_positions(pos_list):
global times
if (len(pos_list) != len(IDS)):
print("pos_list doesn't have the same dimension as ids_list")
return -1
for i in range(len(pos_list)):
# write goal position
dxl_addparam_result = dynamixel.groupSyncWriteAddParam(
GROUP_NUM, IDS[i], pos_list[i] + HOME, LEN_MX_GOAL_POSITION)
if dxl_addparam_result != 1:
print(dxl_addparam_result)
"""dynamixel.write2ByteTxRx(PORT_NUM, PROTOCOL_VERSION, IDS[i], ADDR_MX_GOAL_POSITION, pos_list[i] + HOME)
dxl_comm_result = dynamixel.getLastTxRxResult(PORT_NUM, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(PORT_NUM, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
return -1
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
return -1"""
tv_gswtp = time()
dynamixel.groupSyncWriteTxPacket(GROUP_NUM)
times = helpers.dict_timer("DT.groupSyncWriteTxPacket", times,
time() - tv_gswtp)
# if dynamixel.getLastTxRxResult(PORT_NUM, PROTOCOL_VERSION) != COMM_SUCCESS:
# dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(PORT_NUM, PROTOCOL_VERSION))
tv_gswcp = time()
dynamixel.groupSyncWriteClearParam(GROUP_NUM)
times = helpers.dict_timer("DT.groupSyncWriteClearParam", times,
time() - tv_gswcp)
def set_target_positions(pos_list):
if (len(pos_list) != len(IDS)):
print("pos_list doesn't have the same dimension as ids_list")
return -1
for i in range(len(pos_list)):
# write goal position
dxl_addparam_result = ctypes.c_ubyte(
dynamixel.groupSyncWriteAddParam(GROUP_NUM, IDS[i],
pos_list[i] + HOME,
LEN_MX_GOAL_POSITION)).value
if dxl_addparam_result != 1:
print(dx1_addparam_result)
"""dynamixel.write2ByteTxRx(PORT_NUM, PROTOCOL_VERSION, IDS[i], ADDR_MX_GOAL_POSITION, pos_list[i] + HOME)
dxl_comm_result = dynamixel.getLastTxRxResult(PORT_NUM, PROTOCOL_VERSION)
dxl_error = dynamixel.getLastRxPacketError(PORT_NUM, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
return -1
elif dxl_error != 0:
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
return -1"""
dynamixel.groupSyncWriteTxPacket(GROUP_NUM)
"""if dynamixel.getLastTxRxResult(PORT_NUM, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(PORT_NUM, PROTOCOL_VERSION))"""
dynamixel.groupSyncWriteClearParam(GROUP_NUM)
def set_goal_position(self, ids):
# for i, angle in ids.iteritems():
# self._write(i,angle)
for i, angle in ids.iteritems():
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(self.groupSync, i, 2048 + int(angle / 0.088), self.LEN_MX_GOAL_POSITION)).value
dynamixel.groupSyncWriteTxPacket(self.groupSync)
dxl_comm_result = dynamixel.getLastTxRxResult(self.portHandler, self.PROTOCOL_VERSION)
if(dxl_comm_result != COMM_SUCCESS):
print(dynamixel.getTxRxResult(self.PROTOCOL_VERSION, self.dxl_comm_result))
dynamixel.groupSyncWriteClearParam(self.groupSync)
def group_sync_write_goal(self, commands):
sync_group_num = dynamixel.groupSyncWrite(self.__port_num, PROTOCOL, GOAL_POSITION, 2)
for servo_id, goal_position in commands:
success = dynamixel.groupSyncWriteAddParam(sync_group_num, servo_id, int(goal_position), 2)
if not success:
raise IOError("failed writing to group sync write")
dynamixel.groupSyncWriteTxPacket(sync_group_num)
dxl_comm_result = dynamixel.getLastTxRxResult(self.__port_num, PROTOCOL)
if dxl_comm_result != 0:
error_desciption = get_tx_rx_result_description(dxl_comm_result)
raise IOError("dxl_comm_result: " + error_desciption)
dynamixel.groupSyncWriteClearParam(sync_group_num)
def _syncWrite(self, servos, addr, info_len, values=None):
''' this is an adaptation from dynamixel's sdk for
the sync_write '''
SW = dxl.groupSyncWrite(self.socket, PROTOCOL_VERSION, addr, info_len)
for i, s in enumerate(servos):
if(values is None):
dxl.groupSyncWriteAddParam(SW, s.servo_id, s.goalValue, info_len)
else:
dxl.groupSyncWriteAddParam(SW, s.servo_id, values[i], info_len)
dxl.groupSyncWriteTxPacket(SW) #does the sync write
dxl.groupSyncWriteClearParam(SW) #clears buffer
def _syncWrite(self, servos, addr, info_len, values=None):
''' this is an adaptation from dynamixel's sdk for
the sync_write '''
SW = dxl.groupSyncWrite(self.socket, PROTOCOL_VERSION, addr, info_len)
for i, s in enumerate(servos):
if (values is None):
dxl.groupSyncWriteAddParam(SW, s.servo_id, s.goalValue,
info_len)
else:
dxl.groupSyncWriteAddParam(SW, s.servo_id, values[i], info_len)
dxl.groupSyncWriteTxPacket(SW) #does the sync write
dxl.groupSyncWriteClearParam(SW) #clears buffer
def dxl_torqcon(goal_torq):
# Add Dynamixels goal position value to the Syncwrite storage
for i in xrange(6):#
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(group_num, i+1, goal_torq[i], LEN_MX_GOAL_TORQUE)).value
print(dxl_addparam_result)
if dxl_addparam_result != 1:
print(dxl_addparam_result)
print("[ID:%03d] groupSyncWrite addparam failed" % (i+1))
quit()
# Syncwrite goal position
dynamixel.groupSyncWriteTxPacket(group_num)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
# Clear syncwrite parameter storage
dynamixel.groupSyncWriteClearParam(group_num)
def set_goal_position(self, write_dict):
#self.set_torque_status(write_dict.keys(),1)
for id, angle in write_dict.items():
angle = self.from_degree(angle + 180)
# Add dxl goal position value to the Syncwrite storage
addparam_result = ctypes.c_ubyte(
dxl.groupSyncWriteAddParam(self.groupwrite, id, angle,
LEN_GOAL_POSITION)).value
if addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (id))
quit()
# Syncwrite goal position
dxl.groupSyncWriteTxPacket(self.groupwrite)
self.check_result(id)
# Clear syncwrite parameter storage
dxl.groupSyncWriteClearParam(self.groupwrite)
self.check_result(id)
# Add Dynamixel#1 goal position value to the Syncwrite storage
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(group_num, DXL1_ID, dxl_goal_position[index], LEN_MX_GOAL_POSITION)).value
print(dxl_addparam_result)
if dxl_addparam_result != 1:
print(dxl_addparam_result)
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL1_ID))
quit()
# Add Dynamixel#2 goal position value to the Syncwrite parameter storage
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(group_num, DXL2_ID, dxl_goal_position[index], LEN_MX_GOAL_POSITION)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL2_ID))
# Syncwrite goal position
dynamixel.groupSyncWriteTxPacket(group_num)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
# Clear syncwrite parameter storage
dynamixel.groupSyncWriteClearParam(group_num)
while 1:
# Read Dynamixel#1 present position
dxl1_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_PRESENT_POSITION)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0:
dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION))
# Read Dynamixel#2 present position
print(goalpos)
## Se arma el paquete a enviar al servomotor. Es el envío del comando de posición
## Add Dynamixel#1 goal position value to the Syncwrite storage
dxl_addparam_result = ctypes.c_ubyte(
dynamixel.groupSyncWriteAddParam(group_num, (i + 1),
int(goalpos),
GOAL_POSITION_LEN)).value
print(dxl_addparam_result)
if dxl_addparam_result != 1:
print(dxl_addparam_result)
print("[ID:%03d] groupSyncWrite addparam failed" % (i + 1))
quit()
## Syncwrite goal position
dynamixel.groupSyncWriteTxPacket(group_num)
if dynamixel.getLastTxRxResult(port_num,
PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(
PROTOCOL_VERSION,
dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
## Clear syncwrite parameter storage
dynamixel.groupSyncWriteClearParam(group_num)
## Se finaliza el conteo iniciado antes del proceso de transmisión del paquete de estadosfunción para finalilzar un conteo
t_fin = time.time()
while ((t_fin - t_ini) * 1000 < 100):
t_fin = time.time()
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL3_ID))
quit()
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_acceleration_limit, DXL4_ID, dxl_acc_limit, LEN_XM430_ACCELERATION_LIMIT)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL4_ID))
quit()
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_acceleration_limit, DXL5_ID, dxl_acc_limit, LEN_XM430_ACCELERATION_LIMIT)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL5_ID))
quit()
# Syncwrite acceleration limit
dynamixel.groupSyncWriteTxPacket(groupwrite_acceleration_limit)
if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS:
dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION))
# Clear syncwrite parameter storage
dynamixel.groupSyncWriteClearParam(groupwrite_acceleration_limit)
#--------------------------------------------------------------------------------------------------------------#
# Initialize Groupsyncwrite instance
groupwrite_velocity_limit = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_XM430_VELOCITY_LIMIT, LEN_XM430_VELOCITY_LIMIT)
# Add Dynamixel #1~#5 velocity limit value to the Syncwrite storage
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_velocity_limit, DXL1_ID, dxl_vel_limit, LEN_XM430_VELOCITY_LIMIT)).value
if dxl_addparam_result != 1:
