Dynamixel SDKを使ってPythonでXL330を弄る その２：標準入力から受けて実行

実際のところ

#!/usr/bin/env python
# -*- coding: utf-8 -*-
from dynamixel_sdk import * # Uses Dynamixel SDK library

import msvcrt # for getch
import time
import sys


#********* DYNAMIXEL Model definition *********
MY_DXL = 'X_SERIES'       # X330 (5.0 V recommended), X430, X540, 2X430

ADDR_TORQUE_ENABLE          = 64
ADDR_GOAL_POSITION          = 116
ADDR_PRESENT_POSITION       = 132
DXL_MINIMUM_POSITION_VALUE  = 0         # Refer to the Minimum Position Limit of product eManual
DXL_MAXIMUM_POSITION_VALUE  = 4095      # Refer to the Maximum Position Limit of product eManual
BAUDRATE                    = 57600

LEN_LED_RED                 = 1         # Data Byte Length
LEN_GOAL_POSITION           = 4         # Data Byte Length


# DYNAMIXEL Protocol Version (1.0 / 2.0)
# https://emanual.robotis.com/docs/en/dxl/protocol2/
PROTOCOL_VERSION            = 2.0

# Factory default ID of all DYNAMIXEL is 1
DXL_ID                      = 1

# Use the actual port assigned to the U2D2.
# ex) Windows: "COM*", Linux: "/dev/ttyUSB*", Mac: "/dev/tty.usbserial-*"
DEVICENAME                  = 'COM5'

TORQUE_ENABLE               = 1     # Value for enabling the torque
TORQUE_DISABLE              = 0     # Value for disabling the torque
DXL_MOVING_STATUS_THRESHOLD = 20    # Dynamixel moving status threshold



# 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)

# 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()


try:
    dxl_goal_position = ""
    # Enable Dynamixel Torque
    dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_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 has been successfully connected")

    print('_____________')
    while 1:
        dxl_goal_position = sys.stdin.readline()
        dxl_goal_position = dxl_goal_position.rstrip('\n')
        dxl_goal_position = int(dxl_goal_position)

        # Read present position
        dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL_ID, ADDR_PRESENT_POSITION)
        dxl_comm_result, dxl_error  = packetHandler.write4ByteTxRx(portHandler, DXL_ID, ADDR_GOAL_POSITION , dxl_goal_position)

        print("[ID:%03d] PresPos:%03d" % (DXL_ID, dxl_present_position))
        time.sleep(0.2) #5Hz


except:
    # Disable Dynamixel Torque
    dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_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()

実行はPowerShell7 on Windows10

$ python.exe .\DYNAMIXELTEST.py
Succeeded to open the port
Succeeded to change the baudrate
Dynamixel has been successfully connected
_____________
0
[ID:001] PresPos:001
100
[ID:001] PresPos:001
1000
[ID:001] PresPos:093
450
[ID:001] PresPos:997
200
[ID:001] PresPos:454
2000
[ID:001] PresPos:206
0
[ID:001] PresPos:1997
q
$ 

参考もと