text: #!/usr/bin/env python
  # -*- coding: utf-8 -*-
  
  #
  # multi_port.py
  #
  #  Created on: 2016. 6. 16.
  #      Author: Ryu Woon Jung (Leon)
  #
  
  #
  # *********     MultiPort Example      *********
  #
  #
  # Available Dynamixel model on this example : All models using Protocol 2.0
  # This example is designed for using two Dynamixel PRO 54-200, and two USB2DYNAMIXELs.
  # To use another Dynamixel model, such as X series, see their details in E-Manual(support.robotis.com) and edit below variables yourself.
  # Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 3 (Baudrate : 1000000)
  #
  
  import os
  
  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
  
  os.sys.path.append('../dynamixel_functions_py')             # Path setting
  
  import dynamixel_functions as dynamixel                     # Uses Dynamixel SDK library
  
  # Control table address
  ADDR_PRO_TORQUE_ENABLE       = 562                          # Control table address is different in Dynamixel model
  ADDR_PRO_GOAL_POSITION       = 596
  ADDR_PRO_PRESENT_POSITION    = 611
  
  # Protocol version
  PROTOCOL_VERSION            = 2                             # See which protocol version is used in the Dynamixel
  
  # Default setting
  DXL1_ID                     = 1                             # Dynamixel ID: 1
  DXL2_ID                     = 2                             # Dynamixel ID: 2
  BAUDRATE                    = 1000000
  DEVICENAME1                 = "/dev/ttyUSB0".encode('utf-8')# Check which port is being used on your controller
  DEVICENAME2                 = "/dev/ttyUSB1".encode('utf-8')# ex) Windows: "COM1"   Linux: "/dev/ttyUSB0"
  
  TORQUE_ENABLE               = 1                             # Value for enabling the torque
  TORQUE_DISABLE              = 0                             # Value for disabling the torque
  DXL_MINIMUM_POSITION_VALUE  = -150000                       # Dynamixel will rotate between this value
  DXL_MAXIMUM_POSITION_VALUE  = 150000                        # 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.)
  DXL_MOVING_STATUS_THRESHOLD = 10                            # Dynamixel moving status threshold
  
  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_num1 = dynamixel.portHandler(DEVICENAME1)
  port_num2 = dynamixel.portHandler(DEVICENAME2)
  
  # Initialize PacketHandler Structs
  dynamixel.packetHandler()
  
  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 port1
  if dynamixel.openPort(port_num1):
      print("Succeeded to open the port!")
  else:
      print("Failed to open the port!")
      print("Press any key to terminate...")
      getch()
      quit()
  
  # Open port2
  if dynamixel.openPort(port_num2):
      print("Succeeded to open the port!")
  else:
      print("Failed to open the port!")
      print("Press any key to terminate...")
      getch()
      quit()
  
  
  # Set port1 baudrate
  if dynamixel.setBaudRate(port_num1, BAUDRATE):
      print("Succeeded to change the baudrate!")
  else:
      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_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
  if dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION) != COMM_SUCCESS:
      dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION))
  elif dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION) != 0:
      dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION))
  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)
  if dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION) != COMM_SUCCESS:
      dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION))
  elif dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION) != 0:
      dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num2, 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
  
      # Write Dynamixel#1 goal position
      dynamixel.write4ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index])
      if dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION) != COMM_SUCCESS:
          dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION))
      elif dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION) != 0:
          dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION))
  
      # Write Dynamixel#2 goal position
      dynamixel.write4ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index])
      if dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION) != COMM_SUCCESS:
          dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION))
      elif dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION) != 0:
          dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION))
  
      while 1:
          # Read present position
          dxl1_present_position = dynamixel.read4ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_PRESENT_POSITION)
          if dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION) != COMM_SUCCESS:
              dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION))
          elif dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION) != 0:
              dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION))
  
          # Read present position
          dxl2_present_position = dynamixel.read4ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_PRESENT_POSITION)
          if dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION) != COMM_SUCCESS:
              dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION))
          elif dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION) != 0:
              dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION))
  
          print("[ID:%03d] GoalPos:%03d  PresPos:%03d\t[ID:%03d] GoalPos:%03d  PresPos:%03d" % (DXL1_ID, dxl_goal_position[index], dxl1_present_position, DXL2_ID, dxl_goal_position[index], dxl2_present_position))
  
          if not ((abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) or (abs(dxl_goal_position[index] - dxl2_present_position) > DXL_MOVING_STATUS_THRESHOLD)):
              break
  
      # Change goal position
      if index == 0:
          index = 1
      else:
          index = 0
  
  
  # Disable Dynamixel#1 Torque
  dynamixel.write1ByteTxRx(port_num1, PROTOCOL_VERSION, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
  if dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION) != COMM_SUCCESS:
      dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num1, PROTOCOL_VERSION))
  elif dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION) != 0:
      dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num1, PROTOCOL_VERSION))
  
  # Disable Dynamixel#2 Torque
  dynamixel.write1ByteTxRx(port_num2, PROTOCOL_VERSION, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
  if dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION) != COMM_SUCCESS:
      dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num2, PROTOCOL_VERSION))
  elif dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION) != 0:
      dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num2, PROTOCOL_VERSION))
  
  # Close port
  dynamixel.closePort(port_num1)
  
  dynamixel.closePort(port_num2)
  