def init_motors(ids_list, offsets): global PORT_NUM, GROUP_NUM, IDS IDS = ids_list PORT_NUM = dynamixel.portHandler(DEVICENAME) print(PORT_NUM) # initialize PacketHandler structs dynamixel.packetHandler() GROUP_NUM = dynamixel.groupSyncWrite(PORT_NUM, PROTOCOL_VERSION, ADDR_MX_GOAL_POSITION, LEN_MX_GOAL_POSITION) # open port if not dynamixel.openPort(PORT_NUM): print("Failed to open port!") return -1 # set port baudrate if not dynamixel.setBaudRate(PORT_NUM, BAUDRATE): print("Failed to change the baudrate!") return # enable dynamixel torque for DXL_ID in IDS: dynamixel.write1ByteTxRx(PORT_NUM, PROTOCOL_VERSION, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) 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
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, port): self.port = port self.protocol = 1 self.mx_res = MX_RESOLUTION # MX-28 Resolution self.fsr_res = FSR_RESOLUTION #FSR Resolution self.groupwrite = dxl.groupSyncWrite(self.port, self.protocol, ADDR_GOAL_POS, LEN_GOAL_POSITION) # self.groupspeed = dxl.groupSyncWrite(self.port, self.protocol, ADDR_MOV_SPEED, LEN_MOV_SPEED) # self.groupread = dxl.groupSyncRead(self.port, self.protocol, ADDR_PRES_POS, LEN_PRESENT_POSITION) print("Protocol 1 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 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 __init__(self, DeviceName): self.portHandler = dynamixel.portHandler(DeviceName) self.packetHandler = dynamixel.packetHandler() self.dxl_comm_result = COMM_TX_FAIL self.dxl_error = 0 self.ADDR_MX_TORQUE_ENABLE = 64 # Control table address is different in Dynamixel model self.ADDR_MX_GOAL_POSITION = 116 self.ADDR_MX_PRESENT_POSITION = 132 self.ADDR_MX_SPEED = 104 self.ADDR_MX_PRESENT_SPEED = 104 self.dxl_addparam_result = 0 self.LEN_MX_GOAL_POSITION = 4 self.LEN_MX_PRESENT_POSITION = 4 # Protocol version self.PROTOCOL_VERSION = 2 # See which protocol version is used in the Dynamixel self.groupSync = dynamixel.groupSyncWrite(self.portHandler, self.PROTOCOL_VERSION, self.ADDR_MX_GOAL_POSITION, self.LEN_MX_GOAL_POSITION) self.groupSyncSpeed = dynamixel.groupSyncWrite(self.portHandler,self.PROTOCOL_VERSION, self.ADDR_MX_SPEED, self.LEN_MX_GOAL_POSITION) # Default setting self.BAUDRATE = 57600 self.TORQUE_ENABLE = 1 # Value for enabling the torque self.TORQUE_DISABLE = 0 # Value for disabling the torque self.DXL_MINIMUM_POSITION_VALUE = -1000000 # Dynamixel will rotate between this value self.DXL_MAXIMUM_POSITION_VALUE = 1000000 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) self.DXL_MOVING_STATUS_THRESHOLD = 10 if(dynamixel.openPort(self.portHandler)): print "Port open done" else: print("Failed to open port") quit() if(dynamixel.setBaudRate(self.portHandler, self.BAUDRATE)): print "Change baudrate succeeded" else: print "Cannot add baudrate" quit()
## Parametros de la ecuacion de Choset (helix_rolling) amplitud_par = 60 amplitud_impar = 60 offset_par = 0 offset_impar = 0 desfase = 90 dtheta_dn = 10 dtheta_dt = -18 port_num = dynamixel.portHandler(DEVICENAME) dynamixel.packetHandler() ## Inicializa miembros de la estructura de puntero de datos de paquetesInicia group_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, P_GOAL_POSITION_L, GOAL_POSITION_LEN) ## Se abre el puerto para proceder al envío de paquetes if dynamixel.openPort(port_num): print("Succeeded to open the port!") else: print("Failed to open the port!") print("Press any key to terminate...") getch() quit() ## Establece la tasa de transmision if dynamixel.setBaudRate(port_num, BAUDRATE): print("Succeeded to change the baudrate!") else: print("Failed to change the baudrate!")
ESC_ASCII_VALUE = 0x1b 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 group_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_MX_GOAL_POSITION, LEN_MX_GOAL_POSITION) index = 0 dxl_comm_result = COMM_TX_FAIL # Communication 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): print("Succeeded to open the port!") else:
ESC_ASCII_VALUE = 0x1b 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
TORQUE_DISABLE = 0 # Value for disabling the torque DXL_MOVING_STATUS_THRESHOLD = 3 # Dynamixel moving status threshold ESC_ASCII_VALUE = 0x1b COMM_SUCCESS = 0 # Communication Success result value COMM_TX_FAIL = -1001 # Communication Tx Failed port_num = dynamixel.portHandler(DEVICENAME) # Initialize PacketHandler Structs dynamixel.packetHandler() # Initialize Groupsyncwrite instance group_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_MX_GOAL_TORQUE, LEN_MX_GOAL_TORQUE) dxl_comm_result = COMM_TX_FAIL # Communication result def callback(data): rospy.loginfo(data.data) dxl_torqcon(data.data) 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))
getch() quit() # 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))
TORQUE_DISABLE = 0 # Value for disabling the torque DXL_MOVING_STATUS_THRESHOLD = 3 # Dynamixel moving status threshold ESC_ASCII_VALUE = 0x1b COMM_SUCCESS = 0 # Communication Success result value COMM_TX_FAIL = -1001 # Communication Tx Failed port_num = dynamixel.portHandler(DEVICENAME) # Initialize PacketHandler Structs dynamixel.packetHandler() # Initialize Groupsyncwrite instance group_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_MX_GOAL_TORQUE, LEN_MX_GOAL_TORQUE) torque_mode_on = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_MX_TORQUE_CONT, LEN_MX_TORQUE_CTRL) dxl_comm_result = COMM_TX_FAIL # Communication result def callback(data): rospy.loginfo(data.data) dxl_torcon(data.data) def dxl_torcon(goal_torq): # Write goal position dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, 1, ADDR_MX_GOAL_TORQUE, goal_torq[0]) 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))
ESC_ASCII_VALUE = 0x1b 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
ESC_ASCII_VALUE = 0x1b 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 group_num = dynamixel.groupSyncWrite(port_num, PROTOCOL_VERSION, ADDR_MX_GOAL_POSITION, LEN_MX_GOAL_POSITION) index = 0 dxl_comm_result = COMM_TX_FAIL # Communication 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): print("Succeeded to open the port!") else: print("Failed to open the port!") print("Press any key to terminate...")
ESC_ASCII_VALUE = 0x1b 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