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()
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()