[Spot Dynamixel - 3] Dynamixel Example 코드

 

💉

Dynamixel Example Code

Path, import

DynamixelSDK/python/tests 내의 Python read write protocol 1.0.py 를 단계별로 살펴보겠습니다.

os.sys.path.append('../dynamixel_functions_py')             # Path setting

import dynamixel_functions as dynamixel                     # Uses DYNAMIXEL SDK library

• dynamixel_functions_py 패키지내에 dynamixel_functions 를 import 해줍니다.

 

Address / Protocol

# Control table address
ADDR_MX_TORQUE_ENABLE       = 24                            # Control table address is different in Dynamixel model
ADDR_MX_GOAL_POSITION       = 30
ADDR_MX_PRESENT_POSITION    = 36

# Protocol version
PROTOCOL_VERSION            = 1                             # See which protocol version is used in the Dynamixel

• 자신의 모터에 알맞는 protocol 버전과 address를 기입합니다.

• 자세한 정보는 Dynamixel 홈페이지 내 모터 manual에 기입되어 있습니다.⇒ 예를 들어, AX-12 / AX-12A는 다음 위치에 존재합니다.

ROBOTIS e-Manual

Edit on GitHub NOTE : AX-12+ is the improved version of existing AX-12; the design of circuit, material, and wheel gear are specially improved. NOTE : AX-12A is a new version of the AX-12+ with the same performance but more advanced external design. Only the AX-12A is now being sold.

https://emanual.robotis.com/docs/en/dxl/ax/ax-12a/#control-table-data-address

 

Motor Parameter

# Default setting
DXL_ID                      = 1                             # Dynamixel ID: 1
BAUDRATE                    = 1000000
DEVICENAME                  = "/dev/ttyUSB0".encode('utf-8')# Check which port is being used on your controller
                                                            # 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  = 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 = 10                            # Dynamixel moving status threshold

DXL_ID

• 각 모터가 가진 고유한 ID이며, 올바른 설정이 되어 있어야 통신이 가능합니다.

• 모터의 ID 는 위의 소프트웨어(Dynamixel Wizard)를 깔고 컴퓨터와 연결하면 확인할 수 있습니다.

• Dynamixel Wizard 의 설치과정과 자세한 설명은 하단 링크를 참조하세요

  ROBOTIS e-Manual

  Edit on GitHub DYNAMIXEL Wizard 2.0 is an optimized tool for managing DYNAMIXEL's from various operating systems. The following features are provided with DYNAMIXEL Wizard 2.0.

  https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_wizard2/

  

 

DEVICENAME

• 호스트 PC에서 인식하는 Dynamixel 의 이름이며, 운영체제에 따라 차이가 있습니다.

linux

• $ dmesg | grep tty 를 통해 /dev/ttyUSB0 이 연결되었는지 확인합니다.

• 보통 하나의 연결만 있다면 /dev/ttyUSB0 에 우선 연결되며, 2개 이상인 경우 /dev/ttyUSB1 등으로 이름이 변경됩니다.

window

• 제어판 - 장치관리자 - 포트탭에서 확인활 수 있습니다.

• 모터는 각도가 아닌 position value를 이용하여 모터를 제어한다. position value가 어떤 각도에 mapping 되어 있는지 확인 필수

 

 

port number, packetHandler

# 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_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE]         # Goal position

dxl_present_position = 0                                    # Present position

• 이 부분에서는 특별히 설정할 것은 없습니다.

• 위에서 설정한 DEVICENAME에 따라서 port num이 설정되고 패킷 구성 및 패킷 저장에 사용되는 패킷구성에 사용되는 paramerter를 초기화합니다.

 

Setup

# 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...")
    msvcrt.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...")
    msvcrt.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")

• Open port : 컨트롤러가 다이나믹셀과 통신하기 위해 port를 엽니다.

• Set port baudrate : 통신 baudrate를 설정합니다.

• Enable Dynamixel Torquewrite1ByteTxRx() : DXL_ID의 id를 가진 모터에게 TORQUE_ENABLE value 인 1byte를 ADDR_MX_TORQUE_ENABLE address 로 전달합니다.
•  

Read and Write Dynamixel motor

while 1:
    print("Press any key to continue! (or press ESC to quit!)")
    if msvcrt.getch().decode() == chr(ESC_ASCII_VALUE):
        break

    # Write goal position
    dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_GOAL_POSITION, dxl_goal_position[index])
    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))

    while 1:
        # Read present position
        dxl_present_position = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_PRESENT_POSITION)
        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))

        print("[ID:%03d] GoalPos:%03d  PresPos:%03d" % (DXL_ID, dxl_goal_position[index], dxl_present_position))

        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

• Enable Dynamixel Torque와 같은 방식이며 각 함수는 다음과 같은 역할을 수행합니다.write2ByteTxRx() : dxl_goal_position[index] 인 목표 position value를 전달
• read2ByteTxRx() : 현재 position value를 읽어오기

 

Exit

# Disable Dynamixel Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_MX_TORQUE_ENABLE, TORQUE_DISABLE)
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))


# Close port
dynamixel.closePort(port_num)

• Dynamixel Torque를 Disable 상태로 만들고, port를 닫습니다.

 

 

코드 실행시 오류가 날 경우

permission 관련 오류가 날 경우 아래 코드를 한번 실행시키고 진행하면 문제가 해결됩니다.

 

sudo chmod -R 777 /dev/ttyUSB0