def setAngle(na): print('setAngle begins') global targetAngle while True: newTargetAngle = L.getTargets()[1] #print('targetAngle, newTA: ' + str(targetAngle) + str(newTargetAngle)) #if targetAngle changes, send new angle to dynamixel if targetAngle != newTargetAngle: motorValue = mapTargetAngleToMotorValue(newTargetAngle) targetAngle = newTargetAngle #print(' targetAngle', targetAngle) #motorValue = motorValue - 109 ???? #print('newMotorvalue' , motorValue) ??? if motorValue <= 100 or motorValue >= 4000: return # Write goal position dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_MOVING_SPEED, 200) dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_GOAL_POSITION, motorValue) 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 setAngle(self, steerAngle): print('setAngle begins') self.mapTargetAngleToMotorValue(steerAngle) print("Angle: " + str(steerAngle) + " mappedValue: " + str(self.mappedSteerAngle)) if self.mappedSteerAngle <= 100 or self.mappedSteerAngle >= 4000: return # Write goal position dynamixel.write2ByteTxRx(self.port_num, self.PROTOCOL_VERSION, self.DXL_ID, self.ADDR_MX_MOVING_SPEED, 200) dynamixel.write2ByteTxRx(self.port_num, self.PROTOCOL_VERSION, self.DXL_ID, self.ADDR_MX_GOAL_POSITION, int(self.mappedSteerAngle)) 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)) print('setAngle finished')
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) if dynamixel.getLastTxRxResult(port_num, PROTOCOL_1) != COMM_SUCCESS: dynamixel.printTxRxResult(PROTOCOL_1, dynamixel.getLastTxRxResult(port_num, PROTOCOL_1)) return COMM_ERROR elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_1) != 0: dynamixel.printRxPacketError(PROTOCOL_1, dynamixel.getLastRxPacketError(port_num, PROTOCOL_1)) return HARDWARE_COMM_ERROR else: print("Drive mode changed") time.sleep(0.2)
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))
def torque_on(): # Add Dynamixels torque mode on to the Syncwrite storage for i in xrange(6): dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, i+1, ADDR_MX_TORQUE_ON, 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 set_angle_limit(id,cw_angle_limit, ccw_angle_limit,baudrate): """Configure the angle limits of a servo. :param int id: Servo ``ìd`` :param int cw_angle_limit: Clockwise angle limit to be configured :param int ccw_angle_limit: Counter-clockwise angle limit to be configured :param baudrate: Baudrate of the servo to be configured :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 #Write CW angle limit dynamixel.write2ByteTxRx(port_num, PROTOCOL_1 , id, ADDR_CW_ANGLE_LIMIT, cw_angle_limit) if dynamixel.getLastTxRxResult(port_num, PROTOCOL_1 ) != COMM_SUCCESS: dynamixel.printTxRxResult(PROTOCOL_1 , dynamixel.getLastTxRxResult(port_num, PROTOCOL_1 )) return COMM_ERROR elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_1 ) != 0: dynamixel.printRxPacketError(PROTOCOL_1 , dynamixel.getLastRxPacketError(port_num,PROTOCOL_1 )) return HARDWARE_COMM_ERROR else: print("CW angle changed to: %s" % cw_angle_limit) time.sleep(0.5) # Write CCW angle limit dynamixel.write2ByteTxRx(port_num, PROTOCOL_1 , id, ADDR_CCW_ANGLE_LIMIT, ccw_angle_limit) if dynamixel.getLastTxRxResult(port_num, PROTOCOL_1 ) != COMM_SUCCESS: dynamixel.printTxRxResult(PROTOCOL_1 , dynamixel.getLastTxRxResult(port_num, PROTOCOL_1 )) return COMM_ERROR elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_1 ) != 0: dynamixel.printRxPacketError(PROTOCOL_1 , dynamixel.getLastRxPacketError(port_num, PROTOCOL_1 )) return HARDWARE_COMM_ERROR else: print("CCW angle changed to: %s" % ccw_angle_limit) time.sleep(0.5)
def CheckTxRxResult(self): 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)) return False elif dynamixel.getLastRxPacketError(self.port_num, self.PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError( self.PROTOCOL_VERSION, dynamixel.getLastRxPacketError(self.port_num, self.PROTOCOL_VERSION)) return False return True
def set_torque_max(id, percentage,baudrate): """Sets a servo max torque. :param int id: Servo ``ìd`` :param baudrate: Baudrate of the servo to be configured :param int percentage: Torque percentage :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 #Converting percentage to bit value (check dynamixel e-manual for info) if percentage == 100: value = 1023 else: value = int(percentage/0.0977517107) #Writing the value in the address dynamixel.write2ByteTxRx(port_num, PROTOCOL_1 , id, ADDR_MAX_TORQUE, value) if dynamixel.getLastTxRxResult(port_num, PROTOCOL_1 ) != COMM_SUCCESS: dynamixel.printTxRxResult(PROTOCOL_1 , dynamixel.getLastTxRxResult(port_num, PROTOCOL_1 )) return COMM_ERROR elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_1 ) != 0: dynamixel.printRxPacketError(PROTOCOL_1 , dynamixel.getLastRxPacketError(port_num, PROTOCOL_1 )) return HARDWARE_COMM_ERROR else: time.sleep(0.2)
def set_baudrate(id,new_baudrate,baudrate): """Sets a servo baudrate. :param int id: Servo ``ìd`` :param baudrate: Baudrate of the servo to be configured :param int new_baudrate: ``new_baudrate`` to be configured :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 baudnum = {1000000:1,500000:3,400000:4,250000:7 , 200000 :9 , 115200 : 16 , 57600 : 34 , 19200 : 103 , 9600 : 207} value = baudnum[new_baudrate] # Set baudrate dynamixel.write1ByteTxRx(port_num, PROTOCOL_1 , id, ADDR_BAUDRATE, value) if dynamixel.getLastTxRxResult(port_num, PROTOCOL_1 ) != COMM_SUCCESS: dynamixel.printTxRxResult(PROTOCOL_1 , dynamixel.getLastTxRxResult(port_num, PROTOCOL_1 )) return COMM_ERROR elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_1 ) != 0: dynamixel.printRxPacketError(PROTOCOL_1 , dynamixel.getLastRxPacketError(port_num, PROTOCOL_1 )) return HARDWARE_COMM_ERROR else: print("Baudrate changed to : %s" % baudrate) time.sleep(0.2)
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 factory_reset(id, baudrate): """Resets a servo to factory config. :param int id: Servo ``ìd`` :param baudrate: Baudrate of the servo to be configured :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 dynamixel.factoryReset(port_num, PROTOCOL_1, id, OPERATION_MODE) if dynamixel.getLastTxRxResult(port_num, PROTOCOL_1) != COMM_SUCCESS: print("Aborted") dynamixel.printTxRxResult( PROTOCOL_1, dynamixel.getLastTxRxResult(port_num, PROTOCOL_1)) return COMM_ERROR elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_1) != 0: dynamixel.printRxPacketError( PROTOCOL_1, dynamixel.getLastRxPacketError(port_num, PROTOCOL_1)) return HARDWARE_COMM_ERROR # Wait for reset time.sleep(2)
def set_id(id, new_id,baudrate): """Sets a servo ID. :param int id: Servo ``ìd`` :param baudrate: Baudrate of the servo to be configured :param int new_id: ``new_id`` to be configured :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 #Writes the new ID onto the register dynamixel.write1ByteTxRx(port_num, PROTOCOL_1 ,id, ADDR_ID, new_id) if dynamixel.getLastTxRxResult(port_num, PROTOCOL_1 ) != COMM_SUCCESS: dynamixel.printTxRxResult(PROTOCOL_1, dynamixel.getLastTxRxResult(port_num, PROTOCOL_1)) return COMM_ERROR elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_1 ) != 0: dynamixel.printRxPacketError(PROTOCOL_1, dynamixel.getLastRxPacketError(port_num, PROTOCOL_1 )) return HARDWARE_COMM_ERROR else: print("ID changed") time.sleep(0.2)
def dxl_init(): # Initialize PortHandler Structs # Set the port path # Get methods and members of PortHandlerLinux or PortHandlerWindows # 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() # Enable Dynamixel Torque for i in xrange(6): while 1: dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, i + 1, 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 %d has been successfully connected" % (i + 1)) break
# 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() # Try to broadcast ping the Dynamixel dynamixel.broadcastPing(port_num, PROTOCOL_VERSION) if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS: dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) print("Detected Dynamixel : ") for id in range(0, MAX_ID): if ctypes.c_ubyte(dynamixel.getBroadcastPingResult(port_num, PROTOCOL_VERSION, id)).value: print("[ID:%03d]" % (id)) # Close port dynamixel.closePort(port_num)
else: print("Failed to change the baudrate!") print("Press any key to terminate...") getch() quit() # Read present baudrate of the controller print("Now the controller baudrate is : %d" % (dynamixel.getBaudRate(port_num))) # Try factoryreset print("[ID:%03d] Try factoryreset : " % (DXL_ID)) dynamixel.factoryReset(port_num, PROTOCOL_VERSION, DXL_ID, OPERATION_MODE) if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS: print("Aborted") dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) quit() elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0: dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) # Wait for reset print("Wait for reset...") sleep(2) print("[ID:%03d] factoryReset Success!" % (DXL_ID)) # Set controller baudrate to dxl default baudrate if dynamixel.setBaudRate(port_num, FACTORYRST_DEFAULTBAUDRATE): print("Succeed to change the controller baudrate to : %d" % (FACTORYRST_DEFAULTBAUDRATE)) else:
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 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 search(id_search_min, id_search_max, baudrates_search_list): """Search for servos in range of ``id_search_min`` and ``id_search_max`` for all baudrates in ``baudrates_search_list``. :param int id_search_min: ID to start searching. :param int id_search_max: ID to stop pinging. :param list baudrates_search_list: List containing the baudrates that the user want to search. :return: ``found_servos`` list containing the servos found. :rtype: List containing the ``Dynamixel`` object servos""" #SEARCHING IN THE NETWORK # 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 #Declaring the limits of the search init = id_search_min end = id_search_max #List containing the found servos in the network found_servos = [] #Tries to ping in protocols 1 and 2 for protocol in PROTOCOL_VERSIONS: print("Using protocol %s" % str(protocol)) #Loop through all baudrates for baudrate in baudrates_search_list: actual_id = init # 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 time.sleep(0.2) while actual_id <= end: print("Pinging in ID: %s " % actual_id) # Try to ping the Dynamixel # Get Dynamixel model number dxl_model_number = dynamixel.pingGetModelNum(port_num, protocol, actual_id) if dynamixel.getLastTxRxResult(port_num, protocol) != COMM_SUCCESS: dynamixel.printTxRxResult(protocol, dynamixel.getLastTxRxResult(port_num, protocol)) elif dynamixel.getLastRxPacketError(port_num, protocol) != 0: dynamixel.printRxPacketError(protocol, dynamixel.getLastRxPacketError(port_num, protocol)) else: #Case the ping succeeds, creates an servo object and stores it in the found_servos vector servo = Dynamixel() servo.id = actual_id servo.baudrate = baudrate servo.protocol = protocol servo.model = dxl_model_number found_servos.append(servo) actual_id = actual_id + 1 # Close port dynamixel.closePort(port_num) return found_servos
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) if dynamixel.getLastTxRxResult(port_num, PROTOCOL_1) != COMM_SUCCESS: dynamixel.printTxRxResult(PROTOCOL_1, dynamixel.getLastTxRxResult(port_num, PROTOCOL_1)) return COMM_ERROR elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_1) != 0: dynamixel.printRxPacketError(PROTOCOL_1, dynamixel.getLastRxPacketError(port_num, PROTOCOL_1)) return HARDWARE_COMM_ERROR else: #D gain set time.sleep(0.5) # I gain config dynamixel.write1ByteTxRx(port_num, PROTOCOL_1, id, ADDR_I_GAIN, i_gain) if dynamixel.getLastTxRxResult(port_num, PROTOCOL_1) != COMM_SUCCESS: dynamixel.printTxRxResult(PROTOCOL_1, dynamixel.getLastTxRxResult(port_num, PROTOCOL_1)) return COMM_ERROR elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_1) != 0: dynamixel.printRxPacketError(PROTOCOL_1, dynamixel.getLastRxPacketError(port_num, PROTOCOL_1)) return HARDWARE_COMM_ERROR else: #I gain set time.sleep(0.5) # P gain config dynamixel.write1ByteTxRx(port_num, PROTOCOL_1, id, ADDR_P_GAIN, p_gain) if dynamixel.getLastTxRxResult(port_num, PROTOCOL_1) != COMM_SUCCESS: dynamixel.printTxRxResult(PROTOCOL_1, dynamixel.getLastTxRxResult(port_num, PROTOCOL_1)) return COMM_ERROR elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_1) != 0: dynamixel.printRxPacketError(PROTOCOL_1, dynamixel.getLastRxPacketError(port_num, PROTOCOL_1)) return HARDWARE_COMM_ERROR else: #P gain set time.sleep(0.5) print("Gain changed")