text: #!/usr/bin/env python
  # -*- coding: utf-8 -*-
  
  ################################################################################
  # Copyright 2017 ROBOTIS CO., LTD.
  #
  # Licensed under the Apache License, Version 2.0 (the "License");
  # you may not use this file except in compliance with the License.
  # You may obtain a copy of the License at
  #
  #     http://www.apache.org/licenses/LICENSE-2.0
  #
  # Unless required by applicable law or agreed to in writing, software
  # distributed under the License is distributed on an "AS IS" BASIS,
  # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  # See the License for the specific language governing permissions and
  # limitations under the License.
  ################################################################################
  
  # Author: Ryu Woon Jung (Leon)
  
  #
  # *********     Bulk Read and Bulk Write Example      *********
  #
  #
  # Available Dynamixel model on this example : All models using Protocol 2.0
  # This example is tested with two Dynamixel PRO 54-200, and an USB2DYNAMIXEL
  # Be sure that Dynamixel PRO properties are already set as %% ID : 1 and 2 / Baudnum : 1 (Baudrate : 57600)
  #
  
  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
  
  from dynamixel_sdk import *                    # Uses Dynamixel SDK library
  
  # Control table address
  ADDR_PRO_TORQUE_ENABLE      = 64               # Control table address is different in Dynamixel model
  ADDR_PRO_LED_RED            = 65
  ADDR_PRO_GOAL_POSITION      = 116
  ADDR_PRO_PRESENT_POSITION   = 132
  
  # Data Byte Length
  LEN_PRO_LED_RED             = 1
  LEN_PRO_GOAL_POSITION       = 4
  LEN_PRO_PRESENT_POSITION    = 4
  
  # Protocol version
  PROTOCOL_VERSION            = 2.0               # See which protocol version is used in the Dynamixel
  
  # Default setting
  DXL1_ID                     = 71                 # Dynamixel#1 ID : 1
  DXL2_ID                     = 72                 # Dynamixel#1 ID : 2
  BAUDRATE                    = 1000000             # Dynamixel default baudrate : 57600
  DEVICENAME                  = '/dev/ttyUSB0'    # Check which port is being used on your controller
                                                  # ex) Windows: "COM1"   Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
  
  TORQUE_ENABLE               = 1                 # Value for enabling the torque
  TORQUE_DISABLE              = 0                 # Value for disabling the torque
  DXL_MINIMUM_POSITION_VALUE  = 100           # Dynamixel will rotate between this value
  DXL_MAXIMUM_POSITION_VALUE  = 4000            # 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 = 20                # Dynamixel moving status threshold
  
  index = 0
  dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE]        # Goal position
  dxl_led_value = [0x00, 0x01]                                                        # Dynamixel LED value for write
  
  # Initialize PortHandler instance
  # Set the port path
  # Get methods and members of PortHandlerLinux or PortHandlerWindows
  portHandler = PortHandler(DEVICENAME)
  
  # Initialize PacketHandler instance
  # Set the protocol version
  # Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
  packetHandler = PacketHandler(PROTOCOL_VERSION)
  
  # Initialize GroupBulkWrite instance
  groupBulkWrite = GroupBulkWrite(portHandler, packetHandler)
  
  # Initialize GroupBulkRead instace for Present Position
  groupBulkRead = GroupBulkRead(portHandler, packetHandler)
  
  # Open port
  if portHandler.openPort():
      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 portHandler.setBaudRate(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
  dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
  if dxl_comm_result != COMM_SUCCESS:
      print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  elif dxl_error != 0:
      print("%s" % packetHandler.getRxPacketError(dxl_error))
  else:
      print("Dynamixel#%d has been successfully connected" % DXL1_ID)
  
  # Enable Dynamixel#1 Torque
  dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
  if dxl_comm_result != COMM_SUCCESS:
      print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  elif dxl_error != 0:
      print("%s" % packetHandler.getRxPacketError(dxl_error))
  else:
      print("Dynamixel#%d has been successfully connected" % DXL2_ID)
  
  # Add parameter storage for Dynamixel#1 present position
  dxl_addparam_result = groupBulkRead.addParam(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION)
  if dxl_addparam_result != True:
      print("[ID:%03d] groupBulkRead addparam failed" % DXL1_ID)
      quit()
  
  # Add parameter storage for Dynamixel#2 LED value
  dxl_addparam_result = groupBulkRead.addParam(DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED)
  if dxl_addparam_result != True:
      print("[ID:%03d] groupBulkRead addparam failed" % DXL2_ID)
      quit()
  
  while 1:
      print("Press any key to continue! (or press ESC to quit!)")
      if getch() == chr(0x1b):
          break
  
      # Allocate goal position value into byte array
      param_goal_position = [DXL_LOBYTE(DXL_LOWORD(dxl_goal_position[index])), DXL_HIBYTE(DXL_LOWORD(dxl_goal_position[index])), DXL_LOBYTE(DXL_HIWORD(dxl_goal_position[index])), DXL_HIBYTE(DXL_HIWORD(dxl_goal_position[index]))]
  
      # Add Dynamixel#1 goal position value to the Bulkwrite parameter storage
      dxl_addparam_result = groupBulkWrite.addParam(DXL1_ID, ADDR_PRO_GOAL_POSITION, LEN_PRO_GOAL_POSITION, param_goal_position)
      if dxl_addparam_result != True:
          print("[ID:%03d] groupBulkWrite addparam failed" % DXL1_ID)
          quit()
  
      # Add Dynamixel#2 LED value to the Bulkwrite parameter storage
      dxl_addparam_result = groupBulkWrite.addParam(DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED, [dxl_led_value[index]])
      if dxl_addparam_result != True:
          print("[ID:%03d] groupBulkWrite addparam failed" % DXL2_ID)
          quit()
  
      # Bulkwrite goal position and LED value
      dxl_comm_result = groupBulkWrite.txPacket()
      if dxl_comm_result != COMM_SUCCESS:
          print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  
      # Clear bulkwrite parameter storage
      groupBulkWrite.clearParam()
  
      while 1:
          # Bulkread present position and LED status
          dxl_comm_result = groupBulkRead.txRxPacket()
          if dxl_comm_result != COMM_SUCCESS:
              print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  
          # Check if groupbulkread data of Dynamixel#1 is available
          dxl_getdata_result = groupBulkRead.isAvailable(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION)
          if dxl_getdata_result != True:
              print("[ID:%03d] groupBulkRead getdata failed" % DXL1_ID)
              quit()
  
          # Check if groupbulkread data of Dynamixel#2 is available
          dxl_getdata_result = groupBulkRead.isAvailable(DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED)
          if dxl_getdata_result != True:
              print("[ID:%03d] groupBulkRead getdata failed" % DXL2_ID)
              quit()
  
          # Get present position value
          dxl1_present_position = groupBulkRead.getData(DXL1_ID, ADDR_PRO_PRESENT_POSITION, LEN_PRO_PRESENT_POSITION)
  
          # Get LED value
          dxl2_led_value_read = groupBulkRead.getData(DXL2_ID, ADDR_PRO_LED_RED, LEN_PRO_LED_RED)
  
          print("[ID:%03d] Present Position : %d \t [ID:%03d] LED Value: %d" % (DXL1_ID, dxl1_present_position, DXL2_ID, dxl2_led_value_read))
  
          if not (abs(dxl_goal_position[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD):
              break
  
      # Change goal position
      if index == 0:
          index = 1
      else:
          index = 0
  
  # Clear bulkread parameter storage
  groupBulkRead.clearParam()
  
  # Disable Dynamixel#1 Torque
  dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL1_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
  if dxl_comm_result != COMM_SUCCESS:
      print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  elif dxl_error != 0:
      print("%s" % packetHandler.getRxPacketError(dxl_error))
  
  # Disable Dynamixel#2 Torque
  dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL2_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
  if dxl_comm_result != COMM_SUCCESS:
      print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  elif dxl_error != 0:
      print("%s" % packetHandler.getRxPacketError(dxl_error))
  
  # Close port
  portHandler.closePort()
  