Issue:
I have a Dynamixel XW430-T333-R (model 1270) (firmware 48).
I am controlling it using a microcontroller (Parallax Propeller) through a Dynamixel Bridge. Using Operation Mode 5.
Everything is working well. Except control table backup.
I make sure torque is off before sending the command (to servo ID 1):
FF FF FD 00 01 08 00 20 01 43 54 52 4C 16 F5
I get no response at all. It is as if the servo is off.
What am I doing wrong? I can’t confirm that the packet is correct because I can’t figure out how to send it using the Dynamixel Wizard Packet Window. Although I can successfully send the command from the Wizard with Tools->Control Table->Backup, it does not help me diagnose what I am doing wrong.
DYNAMIXEL Servo:
XW430-T333-R
DYNAMIXEL Controller:
Dynamixel Bridge
Software Interface:
no specific library.
Hi.
Can you check if there’s an issue using the Dynamixel SDK?
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import os
import sys
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
# Add parent directory to find dynamixel_sdk
sys.path.append(os.path.join(os.path.dirname(__file__), '..', 'src'))
from dynamixel_sdk import *
# Protocol version
PROTOCOL_VERSION = 2.0
# Default setting
BAUDRATE = 1000000 # Default Baudrate of DYNAMIXEL
DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller
def send_raw_packet():
# Initialize PortHandler instance
portHandler = PortHandler(DEVICENAME)
# Initialize PacketHandler instance
packetHandler = Protocol2PacketHandler()
# 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()
return
# 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()
return
# Create packet
packet = [
0xFF, # H1
0xFF, # H2
0xFD, # H3
0x00, # RSRV
0x01, # Packet ID
0x08, # LEN1
0x00, # LEN2
0x20, # INST
0x01, # P1
0x43, # P2
0x54, # P3
0x52, # P4
0x4C, # P5
0x16, # CRC1
0xF5 # CRC2
]
# Send packet
result = packetHandler.txPacket(portHandler, packet)
if result != COMM_SUCCESS:
print(packetHandler.getTxRxResult(result))
portHandler.closePort()
return
print("Successfully sent packet")
print("Packet:", ' '.join(f'0x{x:02X}' for x in packet))
# Set packet timeout
portHandler.setPacketTimeoutMillis(1000)
# Receive response
rxpacket, result = packetHandler.rxPacket(portHandler, False)
if result != COMM_SUCCESS:
print("\nResponse Error:")
print(packetHandler.getTxRxResult(result))
else:
print("\nReceived response:")
print("Raw bytes:", ' '.join(f'0x{x:02X}' for x in rxpacket))
# Parse response
if len(rxpacket) > PKT_ERROR:
error = rxpacket[PKT_ERROR]
if error != 0:
print("Error status:", packetHandler.getRxPacketError(error))
else:
print("No error in response")
if len(rxpacket) > PKT_PARAMETER0:
params = rxpacket[PKT_PARAMETER0:PKT_PARAMETER0 + rxpacket[PKT_LENGTH_L] - 2]
print("Parameters:", ' '.join(f'0x{x:02X}' for x in params))
# Close port
portHandler.closePort()
print("\nPort closed")
if __name__ == '__main__':
try:
send_raw_packet()
except KeyboardInterrupt:
print("\nProgram stopped by user")
except Exception as e:
print(f"Error occurred: {e}")
I’m really sorry but I do not have the Dynamixel SDK. I don’t have or program with C, Java, or Python. This is the code that I send in “Spin”. I think you will see the basic steps that your
Python code is taking once communication is established with the servo. (I have no problem getting communication going and sending any other command).
PUB BackupDyna 'store the control table to EEPROM backup area of Dynamixel
'load tx buffer
TxBuffer[00]:=$FF
TxBuffer[01]:=$FF
TxBuffer[02]:=$FD
TxBuffer[03]:=$00
TxBuffer[04]:=$01 'id
TxBuffer[05]:=$08 'length1
TxBuffer[06]:=$00 'length2
TxBuffer[07]:=$20 'instruction
TxBuffer[08]:=$01 'param1
TxBuffer[09]:=$43 'param2
TxBuffer[10]:=$54 'param3
TxBuffer[11]:=$52 'param4
TxBuffer[12]:=$4C 'param5
TxBuffer[13]:=$16 'crc
TxBuffer[14]:=$F5 'crc
dyna_length:=15 'set the length of the packet to send
'get things ready
bytefill(@RxBuffer, 0, 20) 'clear the receive buffer by putting a zero in all the bytes from 0 to 19
dyna_mode:=1 'trigger the half duplex serial communication routine to send the data
time:=60000 'set a timeout variable
'start a loop to monitor timeout
repeat until dyna_Mode == 0 or time==0 'serial communication routine clears the dyna_mode varible when finished
time-- 'decrease the timeout variable
'return the result
if RxBuffer[0] == $FF 'if we got a $FF in byte zero, we know we got a response
if RxBuffer[8] == 0 'if we got a 0 in byte 8, it means no error
return true 'return true (-1)
elseif time==0 'we timed out
return 999
return RxBuffer[8] 'otherwise return the error code, or if servo is not present
'return a zero (buffer is empty)
Note: I am able to communicate and control the servo for all other commands.
Can you confirm that when you execute your Python code when attached to a XW430-T333-R (model 1270) (firmware 48) or equivalent that it executes correctly?
Can anyone at Robotis confirm that this function works?
I have a feeling I’m chasing my tail on this one.
As far as I can tell, it simply doesn’t work as described in Protocol 2.
Based on what you’ve said, it sounds like the examples in the manual and the SDK example code we sent you are working fine.
Given that, could the programming language you’re using be unusual or atypical?
Thank you YunWonho for helping me work through this.
The problem turned out to be the timeout between sending the packet and waiting for the response. It was set at 1.5ms. This works well for all other commands, but the Control Table Backup takes longer to execute. Setting the timeout to 45ms allows the extra time needed.
2 Likes
You can make the servo respond immediately by setting the return delay time to 0!
1 Like
It would respond immediately, but only after having done the backup. So that does not help.
I other words, it takes more time for the Control Table Backup command to execute and then send a reply than it does for most other commands. About 40 ms more.
Time delay is added after the command has finished executing.
1 Like