def SetCCWComplianceSlope(self, Id, ComSlopeCCW): try: dynamixel.write1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.CCW_COMPLIANCE_SLOPE, ComSlopeCCW) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) except : print "Oops! Problem writing goal position..."
def SetHighestLimitTemperature(self, Id, highestLimitTemp): try: dynamixel.write1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.HIGHEST_LIMIT_TEMP, highestLimitTemp) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) except : print "Oops! Problem writing..."
def SetAlarmShutdown(self, Id, alarmShutdown): try: dynamixel.write1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.ALARM_SHUTDOWN, alarmShutdown) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) except : print "Oops! Problem writing goal position..."
def writeValue(self, address, value, size=1): if(size==1): dxl.write1ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, \ address, value) elif(size==2): dxl.write2ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, \ address, value)
def SetIGain(self, Id, IGain): try: dynamixel.write1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.CCW_COMPLIANCE_MARGIN, IGain) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) except : print "Oops! Problem writing goal position..."
def enableTorques(self): ''' Enable torque for all motors connected in this port. ''' dxl.write1ByteTxRx(self.socket, PROTOCOL_VERSION, BROADCAST_ID, \ ADDR_MX_TORQUE_ENABLE, 1)
def SetTorqueEnable(self, Id, enable): # Enable Dynamixel Torque try: dynamixel.write1ByteTxRx(self.port_num, Registers.PROTOCOL_VERSION, Id, Registers.TORQUE_ENABLE, enable) if dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError(Registers.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, Registers.PROTOCOL_VERSION)) except : print "Oops! Problem writing..."
def disableTorques(self): ''' Disables torque for all motors connected to this port ''' dxl.write1ByteTxRx(self.socket, PROTOCOL_VERSION, BROADCAST_ID, \ ADDR_MX_TORQUE_ENABLE, 0)
def set_gains(self,kp,ki,kd, num_tries = RETRIES): retry = 0 while(retry < num_tries): dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_P_GAIN, kp) dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_I_GAIN, ki) dcomm_result += dxl.getLastTxRxResult(self.port, PROTOCOL) derror += dxl.getLastRxPacketError(self.port, PROTOCOL) dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_D_GAIN, kd) dcomm_result += dxl.getLastTxRxResult(self.port, PROTOCOL) derror += dxl.getLastRxPacketError(self.port, PROTOCOL) if dcomm_result != 0: print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result)) retry +=1 elif derror != 0: print_v(dxl.getRxPacketError(PROTOCOL, derror)) retry +=1 else: return 0 raise Exception("set_gains failed after {} tries".format(num_tries))
def set_gains(self,kp,ki,kd, num_tries = RETRIES): retry = 0 while(retry < num_tries): dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_P_GAIN, kp) dcomm_result = dxl.getLastTxRxResult(self.port, PROTOCOL) derror = dxl.getLastRxPacketError(self.port, PROTOCOL) dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_I_GAIN, ki) dcomm_result += dxl.getLastTxRxResult(self.port, PROTOCOL) derror += dxl.getLastRxPacketError(self.port, PROTOCOL) dxl.write1ByteTxRx(self.port, PROTOCOL, self.id, ADDR_D_GAIN, kd) dcomm_result += dxl.getLastTxRxResult(self.port, PROTOCOL) derror += dxl.getLastRxPacketError(self.port, PROTOCOL) if dcomm_result != 0: print_v(dxl.getTxRxResult(PROTOCOL, dcomm_result)) retry +=1 elif derror != 0: print_v(dxl.getRxPacketError(PROTOCOL, derror)) retry +=1 else: return 0 print('[COMM_ERR] ' + str(get_caller())) return -1
def disable_torque(self, ids): for i in ids: dynamixel.write1ByteTxRx(self.portHandler, self.PROTOCOL_VERSION, i, self.ADDR_MX_TORQUE_ENABLE, self.TORQUE_DISABLE)
def set_wheel_mode(self, ids, val = 1): for i in ids: dynamixel.write1ByteTxRx(self.portHandler,self.PROTOCOL_VERSION,i,11,val)
print("Failed to change the baudrate!") print("Press any key to terminate...") getch() quit() # Set port2 baudrate if dynamixel.setBaudRate(port_num2, BAUDRATE): print("Succeeded to change the baudrate!") else: print("Failed to change the baudrate!") print("Press any key to terminate...") getch() quit() # Enable Dynamixel#1 Torque dynamixel.write1ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION) dxl_error = dynamixel.getLastRxPacketError(port_num1, 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#1 has been successfully connected") # Enable Dynamixel#2 Torque dynamixel.write1ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION) dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION) if dxl_comm_result != COMM_SUCCESS:
print("Press any key to terminate...") 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() # Enable Dynamixel Torque dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_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 has been successfully connected") while 1: print("Press any key to continue! (or press ESC to quit!)") if getch() == chr(ESC_ASCII_VALUE): break # Write goal position dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index])
def motor_init(self): # Read a keypress and return the resulting character as a byte string. if os.name == 'nt': import msvcrt def getch(): return msvcrt.getch().decode() else: import sys, tty, termios fd = sys.stdin.fileno() old_settings = termios.tcgetattr(fd) def getch(): try: tty.setraw(sys.stdin.fileno()) ch = sys.stdin.read(1) finally: termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) return ch # Control table address ADDR_MX_TORQUE_ENABLE = 24 # 1 byte RW # Control table address is different in Dynamixel model ADDR_MX_GOAL_POSITION = 30 # 2 bytes RW ADDR_MX_PRESENT_POSITION = 36 # 2 bytes R ADDR_MX_MOVING_SPEED = 32 # 2 bytes RW ADDR_MX_PRESENT_SPEED = 38 # 2 bytes R ADDR_MX_PRESENT_LOAD = 40 # 2 bytes R ADDR_MX_PRESENT_VOLTAGE = 42 # 1 byte R ADDR_MX_GOAL_ACCELERATION = 73 # 1 byte RW ADDR_MX_MAX_TORQUE = 14 # 2 bytes RW # PID Controller Address ADDR_MX_DERIVATIVE_GAIN = 26 # 1 byte RW ADDR_MX_INTEGRAL_GAIN = 27 # 1 byte RW ADDR_MX_PROPORTIONAL_GAIN = 28 # 1 byte RW # Protocol version PROTOCOL_VERSION = 1 # See which protocol version is used in the Dynamixel # Default setting DXL_ID = [1, 2, 3, 4] BAUDRATE = 1000000 DEVICENAME = "COM7".encode( 'utf-8') # Check which port is being used on your controller # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" # Resolution is 0.088 degree / Position 0~4095 / TORQUE_ENABLE = 1 # Value for enabling the torque TORQUE_DISABLE = 0 # Value for disabling the torque DXL_MOVING_STATUS_THRESHOLD = 5 # Dynamixel moving status threshold ESC_ASCII_VALUE = 0x1b # for ESC key to escape out from the operation 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() index = 0 dxl_comm_result = COMM_TX_FAIL # Communication result dxl_error = 0 # Dynamixel error dxl_present_position = 0 # Present position #set position P gain, I gain, D gain - all 1 byte - P only 254 for unit communication # Motor 1 dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_DERIVATIVE_GAIN, 254) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_INTEGRAL_GAIN, 30) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_PROPORTIONAL_GAIN, 65) # Motor 2 dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_DERIVATIVE_GAIN, 254) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_INTEGRAL_GAIN, 50) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_PROPORTIONAL_GAIN, 120) # Motor 3 dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_DERIVATIVE_GAIN, 254) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_INTEGRAL_GAIN, 50) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_PROPORTIONAL_GAIN, 120) # Motor 4 dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_DERIVATIVE_GAIN, 254) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_INTEGRAL_GAIN, 50) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_PROPORTIONAL_GAIN, 120) for id in DXL_ID: # Enable Dynamixel Torque - 1byte dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_MAX_TORQUE, 1024) dxl_comm_result = dynamixel.getLastTxRxResult( port_num, PROTOCOL_VERSION) dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) # Initialize Goal Position 2048 dxl_goal_position_init = 2048 dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_MAX_TORQUE, 1024) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2500) time.sleep(5) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, 2048) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, 2048) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, 2048) time.sleep(3) # Disable Dynamixel Torque for id in DXL_ID: dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE) 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)) elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) # Close port dynamixel.closePort(port_num)
def EnableTorque(self, enabled=True): value = self.TORQUE_ENABLE if enabled else self.TORQUE_DISABLE dynamixel.write1ByteTxRx(self.port_num, self.PROTOCOL_VERSION, self.DXL_ID, self.ADDR_PRO_TORQUE_ENABLE, value) self.CheckTxRxResult()
def __write_byte(self, servo_id, address, data): dynamixel.write1ByteTxRx(self.__port_num, PROTOCOL, servo_id, address, data) self.__verify_last_message()
def set_pid_gain(id, d_gain, i_gain, p_gain, baudrate): """Sets the PID Gains. :param int id: Servo ``ìd`` :param baudrate: Baudrate of the servo to be configured :param int d_gain: D Gain :param int i_gain: I Gain :param int p_gain: P Gain :return: --``PORT_ERROR`` case it fails to open the port. --``BAUDRATE_ERROR`` case it fails to change baudrate. --``COMM_ERROR`` case there is a communication error. --``HARDWARE_COMM_ERROR`` case there is a hardware communication error. --``NONE`` case the operation succeeds.""" # Get methods and members of PortHandlerLinux or PortHandlerWindows port_num = dynamixel.portHandler(DEVICENAME) # Initialize PacketHandler Structs dynamixel.packetHandler() # Open port if dynamixel.openPort(port_num): print("Succeeded to open the port!") else: print("Failed to open the port!") return PORT_ERROR # Set port baudrate if dynamixel.setBaudRate(port_num, baudrate): print("Succeeded to change the baudrate!") else: print("Failed to change the baudrate!") return BAUDRATE_ERROR # D gain config dynamixel.write1ByteTxRx(port_num, PROTOCOL_1, id, ADDR_D_GAIN, d_gain) dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_1) dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_1) if dxl_comm_result != COMM_SUCCESS: print(dynamixel.getTxRxResult(PROTOCOL_1, dxl_comm_result)) return COMM_ERROR elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_1, dxl_error)) return HARDWARE_COMM_ERROR else: print("D gain set") time.sleep(0.2) # I gain config dynamixel.write1ByteTxRx(port_num, PROTOCOL_1, id, ADDR_I_GAIN, i_gain) dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_1) dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_1) if dxl_comm_result != COMM_SUCCESS: print(dynamixel.getTxRxResult(PROTOCOL_1, dxl_comm_result)) return COMM_ERROR elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_1, dxl_error)) return HARDWARE_COMM_ERROR else: print("I gain set") time.sleep(0.2) # P gain config dynamixel.write1ByteTxRx(port_num, PROTOCOL_1, id, ADDR_P_GAIN, p_gain) dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_1) dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_1) if dxl_comm_result != COMM_SUCCESS: print(dynamixel.getTxRxResult(PROTOCOL_1, dxl_comm_result)) return COMM_ERROR elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_1, dxl_error)) return HARDWARE_COMM_ERROR else: print("P gain set") time.sleep(0.2) dynamixel.closePort(port_num)
# if not (abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD): # break # Change goal position # if index == 0: # index = 1 # else: # index = 0 rate.sleep() # Disable Dynamixel Torque dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_PRO_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_106_ID, ADDR_106_TORQUE_ENABLE, TORQUE_DISABLE) # Close port dynamixel.closePort(port_num) if __name__ == '__main__': try: init() initDxl106() initDxlPro() control() except rospy.ROSInterruptException: # Disable Dynamixel Torque dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_PRO_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
def disable_bot(ID): # Disable Dynamixel Torque dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE) dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)
else: print("Failed to open the port!") print("Press any key to terminate...") getch() quit() # Seta a baud rate da porta 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() # Set baudrate dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_ID, NEW_DXL_ID) 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("ID CHANGED FROM %s TO : %s" % (DXL_ID, NEW_DXL_ID)) dynamixel.closePort(port_num)
def torque_ctrl(): dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, 1, ADDR_MX_TORQUE_CONT, 1) 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))
def motor_operate(self, theta1_vp, theta2_vp, e, tx, ty, div_pt_x_vp, div_pt_y_vp): # ta1_vp and theta2_vp from list to float type theta1_vp = theta1_vp.tolist() theta2_vp = theta2_vp.tolist() for i in range(len(theta1_vp)): theta1_vp[i] = float(theta1_vp[i][0]) theta2_vp[i] = float(theta2_vp[i][0]) # Read a keypress and return the resulting character as a byte string. if os.name == 'nt': import msvcrt def getch(): return msvcrt.getch().decode() else: import sys, tty, termios fd = sys.stdin.fileno() old_settings = termios.tcgetattr(fd) def getch(): try: tty.setraw(sys.stdin.fileno()) ch = sys.stdin.read(1) finally: termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) return ch # Control table address ADDR_MX_TORQUE_ENABLE = 24 # 1 byte RW # Control table address is different in Dynamixel model ADDR_MX_GOAL_POSITION = 30 # 2 bytes RW ADDR_MX_PRESENT_POSITION = 36 # 2 bytes R ADDR_MX_MOVING_SPEED = 32 # 2 bytes RW ADDR_MX_PRESENT_SPEED = 38 # 2 bytes R ADDR_MX_PRESENT_LOAD = 40 # 2 bytes R ADDR_MX_PRESENT_VOLTAGE = 42 # 1 byte R ADDR_MX_GOAL_ACCELERATION = 73 # 1 byte RW ADDR_MX_MAX_TORQUE = 14 # 2 bytes RW # PID Controller Address ADDR_MX_DERIVATIVE_GAIN = 26 # 1 byte RW ADDR_MX_INTEGRAL_GAIN = 27 # 1 byte RW ADDR_MX_PROPORTIONAL_GAIN = 28 # 1 byte RW # Protocol version PROTOCOL_VERSION = 1 # See which protocol version is used in the Dynamixel # Default setting DXL_ID = [1, 2, 3, 4] BAUDRATE = 1000000 DEVICENAME = "COM7".encode('utf-8') # Check which port is being used on your controller # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" # Resolution is 0.088 degree / Position 0~4095 / TORQUE_ENABLE = 1 # Value for enabling the torque TORQUE_DISABLE = 0 # Value for disabling the torque DXL_MOVING_STATUS_THRESHOLD = 5 # Dynamixel moving status threshold ESC_ASCII_VALUE = 0x1b # for ESC key to escape out from the operation 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() index = 0 dxl_comm_result = COMM_TX_FAIL # Communication result dxl_error = 0 # Dynamixel error dxl_present_position = 0 # Present position theta1 = [] for i in range(len(theta1_vp)): theta1.append(int((180 * theta1_vp[i]) / (0.088 * np.pi)) + theta1_offset - 2048 + dynamixel_offset_90) print('=====================theta1 =======================') print(theta1) theta2 = [] for i in range(len(theta2_vp)): theta2.append(int((180 * theta2_vp[i]) / (0.088 * np.pi)) + theta2_offset - 2048 + 2048) print('=====================theta2=======================') print(theta2) theta3 = [theta3_offset] * len(theta2_vp) #theta4 = [2048] # 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...") 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() #set position P gain, I gain, D gain - all 1 byte - P only 254 for unit communication # Motor 1 dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_DERIVATIVE_GAIN, 254) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_INTEGRAL_GAIN, 30) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_PROPORTIONAL_GAIN, 80) # Motor 2 dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_DERIVATIVE_GAIN, 254) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_INTEGRAL_GAIN, 50) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_PROPORTIONAL_GAIN, 120) # Motor 3 dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_DERIVATIVE_GAIN, 254) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_INTEGRAL_GAIN, 50) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_PROPORTIONAL_GAIN, 120) # Motor 4 dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_DERIVATIVE_GAIN, 254) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_INTEGRAL_GAIN, 50) dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_PROPORTIONAL_GAIN, 120) for id in DXL_ID: # Enable Dynamixel Torque - 1byte dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_MAX_TORQUE, 1024) dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) # Initialize Goal Position 2048 dxl_goal_position_init = 2048 dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_TORQUE_ENABLE, TORQUE_ENABLE) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_MAX_TORQUE, 1024) # Estimate time start_time = time.time() dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, 2200) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, 2200) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, 2048) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2500) time.sleep(2) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, theta1[0]) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, theta2[0]) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, theta3[0]) time.sleep(5) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2038) time.sleep(4) while not (len(theta1) - index) == 0: # Write goal position dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, theta1[index]) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, theta2[index]) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, theta3[index]) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2038) 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)) elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) # write moving speed for id in DXL_ID: dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_MOVING_SPEED, 50) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_MOVING_SPEED, 20) while 1: # Read present position dxl_present_position_theta1 = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_PRESENT_POSITION) dxl_present_position_theta2 = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_PRESENT_POSITION) dxl_present_position_theta3 = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_PRESENT_POSITION) dxl_present_position_theta4 = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_PRESENT_POSITION) 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)) elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) # read voltage dxl_present_voltage = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_PRESENT_VOLTAGE) # read load dxl_present_load = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_PRESENT_LOAD) # read present speed dxl_present_vel = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_PRESENT_SPEED) #print("[ID:%03d] GoalPos:%03d PresPos:%03d PresVolt:%03d PresLoad:%03d PresVel:%03d" % ( #DXL_ID[0], theta2[index], dxl_present_position_theta2, #dxl_present_voltage, dxl_present_load, dxl_present_vel)) #print('e', e.is_set()) if e.is_set() == 0: pass elif e.is_set() != 0: dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2500) time.sleep(5) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, 2200) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, 2200) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, 2048) #contour cam_paper = cv2.VideoCapture(0) ret_paper, frame_paper = cam_paper.read() frame_paper = cv2.flip(frame_paper, -1) cv2.imshow('frame', frame_paper) cv2.waitKey(1) cont_paper = contour_paper.ContourPaper() tx, ty = cont_paper.cont_paper(frame_paper) print('translated tx and ty', tx, ty) #paper move #new space for current theta1 and theta2 to return IKINE new_theta1 = np.zeros([len(theta1) - index, 1]) new_theta2 = np.zeros([len(theta2) - index, 1]) #new space of current theta1 and theta2 in degree theta1_deg = np.zeros([]) theta2_deg = np.zeros([]) theta1_deg = np.delete(theta1_deg, 0) theta2_deg = np.delete(theta2_deg, 0) #current theta1 and theta2, remove past theta value current_theta1 = theta1[index: ] current_theta2 = theta2[index: ] div_pt_x_vp = div_pt_x_vp[index: ] div_pt_y_vp = div_pt_y_vp[index: ] #print('x position, ', div_pt_x_vp) #print('imhere1') for i in range(len(current_theta1)): theta1_deg = np.append(theta1_deg, ((current_theta1[i] - theta1_offset + 2048 - dynamixel_offset_90) * np.pi * 0.088 / 180)) theta2_deg = np.append(theta2_deg, ((current_theta2[i] - theta2_offset) * np.pi * 0.088 / 180)) IK = ikine.InverseKinematics() for i in range(len(current_theta2)): #print('theta1_deg', theta1_deg[i]) new_theta1[i], new_theta2[i] = IK.ikine(np.int(div_pt_x_vp[i]), np.int(div_pt_y_vp[i]), tx, ty) #print('new_theta1, ', new_theta1) new_theta1 = new_theta1.tolist() new_theta2 = new_theta2.tolist() for i in range(len(new_theta1)): new_theta1[i] = float(new_theta1[i][0]) new_theta2[i] = float(new_theta2[i][0]) theta1 = [] for i in range(len(new_theta1)): theta1.append(int((180 * new_theta1[i]) / (0.088 * np.pi)) + theta1_offset - 2048 + dynamixel_offset_90) print('=====================theta1 =======================') print(theta1) theta2 = [] for i in range(len(new_theta2)): theta2.append(int((180 * new_theta2[i]) / (0.088 * np.pi)) + theta2_offset - 2048 + 2048) print('=====================theta2=======================') print(theta2) theta3 = [theta3_offset] * len(new_theta2) index = 0 time.sleep(5) break else: pass if not ((abs(theta1[index] - dxl_present_position_theta1) > DXL_MOVING_STATUS_THRESHOLD) | ((abs(theta2[index] - dxl_present_position_theta2)) > DXL_MOVING_STATUS_THRESHOLD) | ((abs(theta3[index] - dxl_present_position_theta3)) > DXL_MOVING_STATUS_THRESHOLD)) : break # check event #ABOUT TX and TY -> IKINE -> New theta1 and theta2 init_tx = tx init_ty = ty if e.is_set() != 0: print('error') dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[0], ADDR_MX_GOAL_POSITION, theta1[0]) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[1], ADDR_MX_GOAL_POSITION, theta2[0]) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[2], ADDR_MX_GOAL_POSITION, theta3[0]) time.sleep(5) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2038) time.sleep(3) e.clear() else: pass index += 1 dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID[3], ADDR_MX_GOAL_POSITION, 2500) time.sleep(5) # Disable Dynamixel Torque for id in DXL_ID: dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE) 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)) elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) # Close port dynamixel.closePort(port_num) return tx, ty
def initDynamixel(self): # DIFFERENT MODES # JOIN Mode # MOVING SPEED 0-1023 (0.114 rpm) (1023 - 117.07 rpm) # GOAL POSITION 0-4095 (0.088 degree) # MULTI-TURN Mode # MOVING SPEED 0-1023 # GOAL POSITION -28672 - 28672 (0.088 * RESOLUTION DIVIDER) # WHEEL Mode # MOVING SPEED 0-2047 (1024-2047 is CW, 0-1023 is CCW) 10th bit flips direction # NO GOAL POSITION # Initialize PortHandler Structs # Set the port path # Get methods and members of PortHandlerLinux or PortHandlerWindows self.port_num = dynamixel.portHandler(self.DEVICENAME) # Initialize PacketHandler Structs dynamixel.packetHandler() self.index = 0 self.dxl_comm_result = self.COMM_TX_FAIL # Communication result self.dxl_goal_position = [ self.DXL_MINIMUM_POSITION_VALUE, self.DXL_MAXIMUM_POSITION_VALUE ] # Goal position self.dxl_error = 0 # Dynamixel error self.dxl_present_position = 0 # Present position # Open port if dynamixel.openPort(self.port_num): print("Succeeded to open the port!") else: print("Failed to open the port!") print("Press any key to terminate...") #getch() #quit() # Set port baudrate if dynamixel.setBaudRate(self.port_num, self.BAUDRATE): print("Succeeded to change the baudrate!") else: print("Failed to change the baudrate!") print("Press any key to terminate...") #getch() #quit() # Enable Dynamixel Torque dynamixel.write1ByteTxRx(self.port_num, self.PROTOCOL_VERSION, self.DXL_ID, self.ADDR_MX_TORQUE_ENABLE, self.TORQUE_ENABLE) if dynamixel.getLastTxRxResult( self.port_num, self.PROTOCOL_VERSION) != self.COMM_SUCCESS: dynamixel.printTxRxResult( self.PROTOCOL_VERSION, dynamixel.getLastTxRxResult(self.port_num, self.PROTOCOL_VERSION)) elif dynamixel.getLastRxPacketError(self.port_num, self.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError( self.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, self.PROTOCOL_VERSION)) else: print("Dynamixel has been successfully connected") # Set Speed dynamixel.write2ByteTxRx(self.port_num, self.PROTOCOL_VERSION, self.DXL_ID, self.ADDR_MX_MOVING_SPEED, 200) if dynamixel.getLastTxRxResult( self.port_num, self.PROTOCOL_VERSION) != self.COMM_SUCCESS: dynamixel.printTxRxResult( self.PROTOCOL_VERSION, dynamixel.getLastTxRxResult(self.port_num, self.PROTOCOL_VERSION)) elif dynamixel.getLastRxPacketError(self.port_num, self.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError( self.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, self.PROTOCOL_VERSION)) else: print("Dynamixel speed has been set successfully") print('Dynamixel initialized')
def disableTorque(self): ''' Disables torque in this joint ''' dxl.write1ByteTxRx(self.socket, PROTOCOL_VERSION, self.servo_id, \ ADDR_MX_TORQUE_ENABLE, 0)
print("Failed to open the port!") print("Press any key to terminate...") 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() print("Setting operating mode to position") dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_X_OPERATING_MODE, X_OPERATING_MODE_POSITION) # Enable Dynamixel Torque dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_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)) elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) else: print("Dynamixel has been successfully connected") while 1: print("Press any key to continue! (or press ESC to quit!)")
def disable_torque(dxl_id): # Disable Dynamixel Torque dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, dxl_id, ADDR_X_TORQUE_ENABLE, TORQUE_DISABLE)
else: print("Failed to change the controller baudrate") getch() quit() # Read Dynamixel baudnum dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE) 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("[ID:%03d] Dynamixel baudnum is now : %d" % (DXL_ID, dxl_baudnum_read)) # Write new baudnum dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE, NEW_BAUDNUM) 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("[ID:%03d] Set Dynamixel baudnum to : %d" % (DXL_ID, NEW_BAUDNUM)) # Set port baudrate to BAUDRATE if dynamixel.setBaudRate(port_num, BAUDRATE): print("Succeed to change the controller baudrate to : %d" % (BAUDRATE)) else: print("Failed to change the controller baudrate") getch() quit()
def set_secondary_id(self, ids, secid): for id in ids: dxl.write1ByteTxRx(self.port, self.protocol, id, ADDR_SECONDARY_ID, secid) self.check_result(id) self.check_error(id)
print("Press any key to terminate...") getch() quit() # Set port2 baudrate if dynamixel.setBaudRate(port_num2, BAUDRATE): print("Succeeded to change the baudrate!") else: print("Failed to change the baudrate!") print("Press any key to terminate...") getch() quit() # Enable Dynamixel#1 Torque dynamixel.write1ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) dxl_comm_result = dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION) dxl_error = dynamixel.getLastRxPacketError(port_num1, 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#1 has been successfully connected") # Enable Dynamixel#2 Torque dynamixel.write1ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) dxl_comm_result = dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION) dxl_error = dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION) if dxl_comm_result != COMM_SUCCESS: print(dynamixel.getTxRxResult(PROTOCOL_VERSION, dxl_comm_result))
def set_op_mode(self, ids, mode=3): for id in ids: dxl.write1ByteTxRx(self.port, self.protocol, id, ADDR_OP_MODE, mode) self.check_result(id) self.check_error(id)
def reverse_slave(id, reverse_mode_enable, slave_mode_enable, baudrate): """Sets the drive mode. :param int id: Servo ``ìd`` :param baudrate: Baudrate of the servo to be configured :param int reverse_mode_enable: Reverse mode checkbox state :param int slave_mode_enable: Slave mode checkbox state :return: --``PORT_ERROR`` case it fails to open the port. --``BAUDRATE_ERROR`` case it fails to change baudrate. --``COMM_ERROR`` case there is a communication error. --``HARDWARE_COMM_ERROR`` case there is a hardware communication error. --``NONE`` case the operation succeeds.""" # Get methods and members of PortHandlerLinux or PortHandlerWindows port_num = dynamixel.portHandler(DEVICENAME) # Initialize PacketHandler Structs dynamixel.packetHandler() # Open port if dynamixel.openPort(port_num): print("Succeeded to open the port!") else: print("Failed to open the port!") return PORT_ERROR # Set port baudrate if dynamixel.setBaudRate(port_num, baudrate): print("Succeeded to change the baudrate!") else: print("Failed to change the baudrate!") return BAUDRATE_ERROR slave_binary = 0x02 reverse_binary = 0x01 drive_mode_byte = 0x00 if reverse_mode_enable == 2: drive_mode_byte = reverse_binary + drive_mode_byte else: drive_mode_byte = drive_mode_byte if slave_mode_enable == 2: drive_mode_byte = slave_binary + drive_mode_byte else: drive_mode_byte = drive_mode_byte # Set drive mode dynamixel.write1ByteTxRx(port_num, PROTOCOL_1, id, ADDR_DRIVE_MODE, drive_mode_byte) dxl_comm_result = dynamixel.getLastTxRxResult(port_num, PROTOCOL_1) dxl_error = dynamixel.getLastRxPacketError(port_num, PROTOCOL_1) if dxl_comm_result != COMM_SUCCESS: print(dynamixel.getTxRxResult(PROTOCOL_1, dxl_comm_result)) return COMM_ERROR elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_1, dxl_error)) return HARDWARE_COMM_ERROR else: print("Drive mode changed") time.sleep(0.2) dynamixel.closePort(port_num)
def set_torque_status(self, ids, value): for id in ids: dxl.write1ByteTxRx(self.port, self.protocol, id, ADDR_TORQUE_ENABLE, value) self.check_result(id) self.check_error(id)
def Init_Port_And_Motors(self): # Open port if dynamixel.openPort(self.port_num): print("Successfully opened the port!") else: print("Failed to open the port!") print("Press any key to terminate...") getch() quit() # Set port baudrate if dynamixel.setBaudRate(self.port_num, self.baud_rate): print("Successfully set the baudrate!") else: print("Failed to change the baudrate!") print("Press any key to terminate...") getch() quit() ADDR_PRO_TORQUE_ENABLE = 64 TORQUE_ENABLE = 1 dxl_comm_result = COMM_TX_FAIL # Enable Dynamixel#1 Torque dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m1id, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num, self.proto_ver) dxl_error = dynamixel.getLastRxPacketError(self.port_num, self.proto_ver) if dxl_comm_result != COMM_SUCCESS: print(dynamixel.getTxRxResult(self.proto_ver, dxl_comm_result)) elif dxl_error != 0: print(dynamixel.getRxPacketError(self.proto_ver, dxl_error)) else: print("Dynamixel#1 has been successfully connected") # Enable Dynamixel#2 Torque dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m2id, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num, self.proto_ver) dxl_error = dynamixel.getLastRxPacketError(self.port_num, self.proto_ver) if dxl_comm_result != COMM_SUCCESS: print(dynamixel.getTxRxResult(self.proto_ver, dxl_comm_result)) elif dxl_error != 0: print(dynamixel.getRxPacketError(self.proto_ver, dxl_error)) else: print("Dynamixel#2 has been successfully connected") # Enable Dynamixel#3 Torque dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m3id, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num, self.proto_ver) dxl_error = dynamixel.getLastRxPacketError(self.port_num, self.proto_ver) if dxl_comm_result != COMM_SUCCESS: print(dynamixel.getTxRxResult(self.proto_ver, dxl_comm_result)) elif dxl_error != 0: print(dynamixel.getRxPacketError(self.proto_ver, dxl_error)) else: print("Dynamixel#3 has been successfully connected") # Enable Dynamixel#4 Torque dynamixel.write1ByteTxRx(self.port_num, self.proto_ver, self.m4id, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) dxl_comm_result = dynamixel.getLastTxRxResult(self.port_num, self.proto_ver) dxl_error = dynamixel.getLastRxPacketError(self.port_num, self.proto_ver) if dxl_comm_result != COMM_SUCCESS: print(dynamixel.getTxRxResult(self.proto_ver, dxl_comm_result)) elif dxl_error != 0: print(dynamixel.getRxPacketError(self.proto_ver, dxl_error)) else: print("Dynamixel#4 has been successfully connected")
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_XM430_VELOCITY_LIMIT, dxl_vel_limit) # Velocity Limit #2 dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_XM430_VELOCITY_LIMIT, dxl_vel_limit) # Velocity Limit #3 dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL3_ID, ADDR_XM430_VELOCITY_LIMIT, dxl_vel_limit) # Velocity Limit #4 dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL4_ID, ADDR_XM430_VELOCITY_LIMIT, dxl_vel_limit) # Velocity Limit #5 dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL5_ID, ADDR_XM430_VELOCITY_LIMIT, dxl_vel_limit) # Velocity Limit #6 dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL6_ID, ADDR_XM430_VELOCITY_LIMIT, dxl_vel_limit) # Velocity Limit #7 dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL7_ID, ADDR_XM430_VELOCITY_LIMIT, dxl_vel_limit) # Enable Dynamixel#1 Torque dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL1_ID, ADDR_XM430_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#1 has been successfully connected") # Enable Dynamixel#2 Torque dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL2_ID, ADDR_XM430_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")
getch() quit() # Read Dynamixel baudnum dxl_baudnum_read = dynamixel.read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_BAUDRATE) 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)) elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) else: print("[ID:%03d] Dynamixel baudnum is now : %d" % (DXL_ID, dxl_baudnum_read)) # Write new baudnum dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_BAUDRATE, NEW_BAUDNUM) 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)) elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) else: print("[ID:%03d] Set Dynamixel baudnum to : %d" % (DXL_ID, NEW_BAUDNUM)) # Set port baudrate to BAUDRATE if dynamixel.setBaudRate(port_num, BAUDRATE): print("Succeed to change the controller baudrate to : %d" % (BAUDRATE)) else: print("Failed to change the controller baudrate") getch()
def motor_disable(self, theta1_vp, theta2_vp): if os.name == 'nt': import msvcrt def getch(): return msvcrt.getch().decode() else: import sys, tty, termios fd = sys.stdin.fileno() old_settings = termios.tcgetattr(fd) def getch(): try: tty.setraw(sys.stdin.fileno()) ch = sys.stdin.read(1) finally: termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) return ch # Control table address ADDR_MX_TORQUE_ENABLE = 24 # 1 byte RW # Control table address is different in Dynamixel model # Protocol version PROTOCOL_VERSION = 1 # See which protocol version is used in the Dynamixel # Default setting DXL_ID = [1, 2, 3, 4] BAUDRATE = 1000000 DEVICENAME = "COM7".encode( 'utf-8') # Check which port is being used on your controller # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" # Resolution is 0.088 degree / Position 0~4095 / TORQUE_ENABLE = 1 # Value for enabling the torque TORQUE_DISABLE = 0 # Value for disabling the torque DXL_MOVING_STATUS_THRESHOLD = 3 # Dynamixel moving status threshold ESC_ASCII_VALUE = 0x1b # for ESC key to escape out from the operation 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() index = 0 dxl_comm_result = COMM_TX_FAIL # Communication result dxl_error = 0 # Dynamixel error dxl_present_position = 0 # Present position # Disable Dynamixel Torque for id in DXL_ID: dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, id, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE) 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)) elif dxl_error != 0: print(dynamixel.getRxPacketError(PROTOCOL_VERSION, dxl_error)) # Close port dynamixel.closePort(port_num)