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)
  
  #
  # *********     Indirect Address Example      *********
  #
  #
  # Available Dynamixel model on this example : All models using Protocol 2.0
  # This example is tested with a Dynamixel PRO 54-200, and an USB2DYNAMIXEL
  # Be sure that Dynamixel PRO properties are already set as %% ID : 1 / 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
  # Control table address is different in Dynamixel model
  ADDR_PRO_INDIRECTADDRESS_FOR_WRITE      = 168#49        # EEPROM region
  ADDR_PRO_INDIRECTADDRESS_FOR_READ       = 178#59        # EEPROM region
  ADDR_PRO_TORQUE_ENABLE                  = 64#562
  ADDR_PRO_LED_RED                        = 65#563
  ADDR_PRO_GOAL_POSITION                  = 116#596
  ADDR_PRO_MOVING                         = 122#610
  ADDR_PRO_PRESENT_POSITION               = 132#611
  ADDR_PRO_INDIRECTDATA_FOR_WRITE         = 224#634
  ADDR_PRO_INDIRECTDATA_FOR_READ          = 229#639
  
  # Data Byte Length
  LEN_PRO_LED_RED                         = 1
  LEN_PRO_GOAL_POSITION                   = 4
  LEN_PRO_MOVING                          = 1
  LEN_PRO_PRESENT_POSITION                = 4
  LEN_PRO_INDIRECTDATA_FOR_WRITE          = 5
  LEN_PRO_INDIRECTDATA_FOR_READ           = 5
  
  # Protocol version
  PROTOCOL_VERSION            = 2.0                   # See which protocol version is used in the Dynamixel
  
  # Default setting
  DXL_ID                      = 1                     # Dynamixel ID : 1
  BAUDRATE                    = 57600                 # 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_MINIMUM_LED_VALUE       = 0                     #  Dynamixel LED will light between this value
  DXL_MAXIMUM_LED_VALUE       = 1                  #  and this value
  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 = [DXL_MINIMUM_LED_VALUE, DXL_MAXIMUM_LED_VALUE]                      # Dynamixel LED value
  
  # 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 GroupSyncWrite instance
  groupSyncWrite = GroupSyncWrite(portHandler, packetHandler, ADDR_PRO_INDIRECTDATA_FOR_WRITE, LEN_PRO_INDIRECTDATA_FOR_WRITE)
  
  # Initialize GroupSyncRead instace for Present Position
  groupSyncRead = GroupSyncRead(portHandler, packetHandler, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_INDIRECTDATA_FOR_READ)
  
  # 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()
  
  
  # Disable Dynamixel Torque :
  # Indirect address would not accessible when the torque is already enabled
  dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_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))
  else:
      print("[ID:%03d] Dynamixel has been successfully connected" % DXL_ID)
  
  # INDIRECTDATA parameter storages replace LED, goal position, present position and moving status storages
  dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 0, ADDR_PRO_GOAL_POSITION + 0)
  if dxl_comm_result != COMM_SUCCESS:
      print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  elif dxl_error != 0:
      print("%s" % packetHandler.getRxPacketError(dxl_error))
  dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 2, ADDR_PRO_GOAL_POSITION + 1)
  if dxl_comm_result != COMM_SUCCESS:
      print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  elif dxl_error != 0:
      print("%s" % packetHandler.getRxPacketError(dxl_error))
  dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 4, ADDR_PRO_GOAL_POSITION + 2)
  if dxl_comm_result != COMM_SUCCESS:
      print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  elif dxl_error != 0:
      print("%s" % packetHandler.getRxPacketError(dxl_error))
  dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 6, ADDR_PRO_GOAL_POSITION + 3)
  if dxl_comm_result != COMM_SUCCESS:
      print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  elif dxl_error != 0:
      print("%s" % packetHandler.getRxPacketError(dxl_error))
  dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_WRITE + 8, ADDR_PRO_LED_RED)
  if dxl_comm_result != COMM_SUCCESS:
      print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  elif dxl_error != 0:
      print("%s" % packetHandler.getRxPacketError(dxl_error))
  
  dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 0, ADDR_PRO_PRESENT_POSITION + 0)
  if dxl_comm_result != COMM_SUCCESS:
      print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  elif dxl_error != 0:
      print("%s" % packetHandler.getRxPacketError(dxl_error))
  dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 2, ADDR_PRO_PRESENT_POSITION + 1)
  if dxl_comm_result != COMM_SUCCESS:
      print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  elif dxl_error != 0:
      print("%s" % packetHandler.getRxPacketError(dxl_error))
  dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 4, ADDR_PRO_PRESENT_POSITION + 2)
  if dxl_comm_result != COMM_SUCCESS:
      print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  elif dxl_error != 0:
      print("%s" % packetHandler.getRxPacketError(dxl_error))
  dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 6, ADDR_PRO_PRESENT_POSITION + 3)
  if dxl_comm_result != COMM_SUCCESS:
      print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  elif dxl_error != 0:
      print("%s" % packetHandler.getRxPacketError(dxl_error))
  dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_INDIRECTADDRESS_FOR_READ + 8, ADDR_PRO_MOVING)
  if dxl_comm_result != COMM_SUCCESS:
      print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  elif dxl_error != 0:
      print("%s" % packetHandler.getRxPacketError(dxl_error))
  
  # Enable Dynamixel Torque
  dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_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))
  
  
  # Add parameter storage for multiple values
  dxl_addparam_result = groupSyncRead.addParam(DXL_ID)
  if dxl_addparam_result != True:
      print("[ID:%03d] groupSyncRead addparam failed" % DXL_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_indirect_data_for_write = [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]))]
      param_indirect_data_for_write.append(dxl_led_value[index])
  
      # Add values to the Syncwrite parameter storage
      dxl_addparam_result = groupSyncWrite.addParam(DXL_ID, param_indirect_data_for_write)
      if dxl_addparam_result != True:
          print("[ID:%03d]groupSyncWrite addparam failed" % DXL_ID)
          quit()
  
      # Syncwrite all
      dxl_comm_result = groupSyncWrite.txPacket()
      if dxl_comm_result != COMM_SUCCESS:
          print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  
      # Clear syncwrite parameter storage
      groupSyncWrite.clearParam()
  
      while 1:
          # Syncread present position from indirectdata2
          dxl_comm_result = groupSyncRead.txRxPacket()
          if dxl_comm_result != COMM_SUCCESS:
              print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
  
          # Check if groupsyncread data of Dynamixel present position value is available
          dxl_getdata_result = groupSyncRead.isAvailable(DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION)
          if dxl_getdata_result != True:
              print("[ID:%03d] groupSyncRead getdata failed" % DXL_ID)
              quit()
  
          # Check if groupsyncread data of Dynamixel moving status is available
          dxl_getdata_result = groupSyncRead.isAvailable(DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING)
          if dxl_getdata_result != True:
              print("[ID:%03d] groupSyncRead getdata failed" % DXL_ID)
              quit()
  
          # Get Dynamixel present position value
          dxl_present_position = groupSyncRead.getData(DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ, LEN_PRO_PRESENT_POSITION)
  
          # Get Dynamixel moving status value
          dxl_moving = groupSyncRead.getData(DXL_ID, ADDR_PRO_INDIRECTDATA_FOR_READ + LEN_PRO_PRESENT_POSITION, LEN_PRO_MOVING)
  
          print("[ID:%03d] GoalPos:%d  PresPos:%d  IsMoving:%d" % (DXL_ID, dxl_goal_position[index], dxl_present_position, dxl_moving))
  
          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
  
  # Clear syncread parameter storage
  groupSyncRead.clearParam()
  
  # Disable Dynamixel Torque
  dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_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()
  