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 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 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)
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE)
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))
else:
print("Dynamixel#2 has been successfully connected")
while 1:
print("Press any key to continue! (or press ESC to quit!)")
if getch() == chr(ESC_ASCII_VALUE):
break
# 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))
## Se calcula el ángulo para el número de servomotor en el tiempo determinado
angulo = Choset(amplitud_par, amplitud_impar, desfase, dtheta_dn,
dtheta_dt, (muestra + (10 * (t_time))), i,
offset_par, offset_impar)
## Si el módulo se seleccionó como inactivo, el ángulo se vuelve cero.
angulo *= MOD_ACTIVOS[i]
goalpos = 512 - angulo * 3.41
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))
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))
else:
print("Dynamixel#2 has been successfully connected")
while 1:
print("Press any key to continue! (or press ESC to quit!)")
if getch() == chr(ESC_ASCII_VALUE):
break
# 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))
# Add parameter storage for Dynamixel#7 present position value
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncReadAddParam(groupread_num, DXL7_ID)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncRead addparam failed" % (DXL7_ID))
quit()
while 1:
print("Press any key to continue! (or press ESC to quit!)")
if getch() == chr(ESC_ASCII_VALUE):
break
for pose in range(0, 10):
# Add Dynamixel#1 goal position value to the Syncwrite storage
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL1_ID, dxl_goal_position[pose][DXL1_ID - 1], LEN_XM430_GOAL_POSITION)).value
if dxl_addparam_result != 1:
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(groupwrite_num, DXL2_ID, dxl_goal_position[pose][DXL2_ID - 1], LEN_XM430_GOAL_POSITION)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL2_ID))
quit()
# Add Dynamixel#3 goal position value to the Syncwrite parameter storage
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL3_ID, dxl_goal_position[pose][DXL3_ID - 1], LEN_XM430_GOAL_POSITION)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL3_ID))
quit()
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE)
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))
else:
print("Dynamixel#2 has been successfully connected")
while 1:
print("Press any key to continue! (or press ESC to quit!)")
if getch() == chr(ESC_ASCII_VALUE):
break
# 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))
print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error))
# Add parameter storage for Dynamixel 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))
quit()
while 1:
print("Press any key to continue! (or press ESC to quit!)")
if getch() == chr(ESC_ASCII_VALUE):
break
# Add Dynamixel#1 goal position value to the Syncwrite storage
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL_ID, dxl_goal_position[index], LEN_PRO_GOAL_POSITION)).value
print(dxl_addparam_result)
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL_ID))
quit()
# Add Dynamixel#2 goal position value to the Syncwrite parameter storage
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL_ID, dxl_led_value[index], LEN_PRO_LED_RED)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL_ID))
quit()
# Syncwrite goal position
dynamixel.groupSyncWriteTxPacket(groupwrite_num)
dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)
if dxl_comm_result != COMM_SUCCESS:
# Set port baudrate
if dynamixel.setBaudRate(port_num, BAUDRATE):
print("Succeeded to change the baudrate!")
else:
print("Failed to change the baudrate!")
print("Press any key to terminate...")
getch()
quit()
# Initialize Groupsyncwrite instance
groupwrite_acceleration_limit = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_XM430_ACCELERATION_LIMIT, LEN_XM430_ACCELERATION_LIMIT)
# Add Dynamixel #1~#5 acceleration limit value to the Syncwrite storage
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_acceleration_limit, DXL1_ID, dxl_acc_limit, LEN_XM430_ACCELERATION_LIMIT)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL1_ID))
quit()
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_acceleration_limit, DXL2_ID, dxl_acc_limit, LEN_XM430_ACCELERATION_LIMIT)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL2_ID))
quit()
dxl_addparam_result = ctypes.c_ubyte(dynamixel.groupSyncWriteAddParam(groupwrite_acceleration_limit, DXL3_ID, dxl_acc_limit, LEN_XM430_ACCELERATION_LIMIT)).value
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
# Add parameter storage for Dynamixel 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))
quit()
while 1:
print("Press any key to continue! (or press ESC to quit!)")
if getch() == chr(ESC_ASCII_VALUE):
break
# Add Dynamixel#1 goal position value to the Syncwrite storage
dxl_addparam_result = ctypes.c_ubyte(
dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL_ID,
dxl_goal_position[index],
LEN_PRO_GOAL_POSITION)).value
print(dxl_addparam_result)
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL_ID))
quit()
# Add Dynamixel#2 goal position value to the Syncwrite parameter storage
dxl_addparam_result = ctypes.c_ubyte(
dynamixel.groupSyncWriteAddParam(groupwrite_num, DXL_ID,
dxl_led_value[index],
LEN_PRO_LED_RED)).value
if dxl_addparam_result != 1:
print("[ID:%03d] groupSyncWrite addparam failed" % (DXL_ID))
quit()
