MX-64T programming issue related to goal_velocity

Technical Support
, , ,
1

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
ADDR_MX_GOAL_VELOCITY = 104
ADDR_MX_PRESENT_POSITION = 132

Data Byte Length

LEN_MX_GOAL_VELOCITY = 4
LEN_MX_PRESENT_POSITION = 4

Protocol version

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

Default setting

DXL1_ID = 1 # Dynamixel#1 ID : 1
BAUDRATE = 57600 # Dynamixel default baudrate : 57600
DEVICENAME = ‘COM4’ # 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_velocity = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position

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 GroupBulkRead instace for Present Position

groupBulkRead = GroupBulkRead(portHandler, packetHandler)

Initialize GroupSyncWrite instance

groupSyncWrite = GroupSyncWrite(portHandler, packetHandler, ADDR_MX_GOAL_VELOCITY, LEN_MX_GOAL_VELOCITY)

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)

Add parameter storage for Dynamixel#1 present position

dxl_addparam_result = groupBulkRead.addParam(DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION)
if dxl_addparam_result != True:
print(“[ID:%03d] groupBulkRead addparam failed” % DXL1_ID)
quit()

while 1:
print(“Press any key to continue! (or press ESC to quit!)”)
if getch() == chr(0x1b):
break

param_goal_velocity = [DXL_LOBYTE(DXL_LOWORD(dxl_goal_velocity[index])), DXL_HIBYTE(DXL_LOWORD(dxl_goal_velocity[index])), DXL_LOBYTE(DXL_HIWORD(dxl_goal_velocity[index])), DXL_HIBYTE(DXL_HIWORD(dxl_goal_velocity[index]))]

# Add Dynamixel#1 goal position value to the Syncwrite parameter storage
dxl_addparam_result = groupSyncWrite.addParam(DXL1_ID, param_goal_velocity)
if dxl_addparam_result != True:
    print("[ID:%03d] groupSyncWrite addparam failed" % DXL1_ID)
    quit()

# Syncwrite goal position
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:
    # Bulkread present position and moving 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_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION)
    if dxl_getdata_result != True:
        print("[ID:%03d] groupBulkRead getdata failed" % DXL1_ID)
        quit()

    # Get Dynamixel#1 present position value
    dxl1_present_position = groupBulkRead.getData(DXL1_ID, ADDR_MX_PRESENT_POSITION, LEN_MX_PRESENT_POSITION)

    print("[ID:%03d] GoalPos:%03d  PresPos:%03d" % (DXL1_ID, dxl_goal_velocity[index], dxl1_present_position))

    if not ((abs(dxl_goal_velocity[index] - dxl1_present_position) > DXL_MOVING_STATUS_THRESHOLD) ):
        break

# Change goal position
if index == 0:
    index = 1
else:
    index = 0

Clear syncread 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))

Close port

portHandler.closePort()

  1. Model of servo: MX-64T

  2. Dynamixel wizard 2.0 and Rplus manager

  3. I want to run motor goal_velocity instead of goal_position . i have attached the code kindly help me to correct this code, because by this code my motor run continuesly with constant speed.

2

I’m not 100% certain that I’m understanding the question you are asking. Would you be able to clarify what kind of assistance you are requesting, so that I can provide you with the appropriate assistance?