GroupSyncRead with 6 servo limit?

Strange problem. I can only get 6 servo to work. The system is 7 STS3215 connecting with Waveshare Bus Servo adaptor.

```bash 
(lerobot) david@WUJIE14-PRO:~/Works/lerobot$ python my-notes/group-sync-read.py --n 7
Succeeded to open the port /dev/ttyACM0
Succeeded to change the baudrate
Press any key to read position! (or press ESC to quit)
Failed to sync read: [TxRxResult] Incorrect status packet!
Press any key to read position! (or press ESC to quit)

(lerobot) david@WUJIE14-PRO:~/Works/lerobot$ python my-notes/group-sync-read.py --n 6
Succeeded to open the port /dev/ttyACM0
Succeeded to change the baudrate
Press any key to read position! (or press ESC to quit)

Press any key to read position! (or press ESC to quit)
```

# System Info
```bash
(lerobot) david@WUJIE14-PRO:~/Works/lerobot$ lsb_release -a
No LSB modules are available.
Distributor ID:	Ubuntu
Description:	Ubuntu 22.04.5 LTS
Release:	22.04
Codename:	jammy
(lerobot) david@WUJIE14-PRO:~/Works/lerobot$ hostnamectl
 Static hostname: WUJIE14-PRO
       Icon name: computer-laptop
         Chassis: laptop
      Machine ID: 9b32efb5a41f43b591390fd01f62884a
         Boot ID: 287d931495df40de9e376cff6b053047
Operating System: Ubuntu 22.04.5 LTS              
          Kernel: Linux 6.8.0-57-generic
    Architecture: x86-64
 Hardware Vendor: MECHREVO
  Hardware Model: WUJIE14 PRO
```

```python
# https://www.genspark.ai/agents?id=992db78d-0889-4f1e-9c39-a0c8fa13ffd4

import os
import argparse

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 scservo_sdk import *                    # Uses SCServo SDK library

# Control table addresses for the servo
ADDR_STS_PRESENT_POSITION = 56   # Address for Present Position
ADDR_STS_TORQUE_ENABLE = 64      # Address for Torque Enable status
PRESENT_POSITION_SIZE = 4        # Data size is 4 bytes (position + speed)
TORQUE_ENABLE_SIZE = 1           # Data size is 1 byte for torque status

# Set up command line arguments
parser = argparse.ArgumentParser(description='SCServo group sync read utility')
parser.add_argument('--port', type=str, default='/dev/ttyACM0',
                   help='Port name (e.g. /dev/ttyACM0 or COM1)')
parser.add_argument('--n', type=int, default=7,
                   help='Number of servos to check (1-n)')
args = parser.parse_args()

# Protocol settings
BAUDRATE = 1000000              # Default Baudrate of SCServo
PROTOCOL_END = 0                # SCServo bit end(STS/SMS=0, SCS=1)

# Set the port path
portHandler = PortHandler(args.port)

# Initialize PacketHandler instance
packetHandler = PacketHandler(PROTOCOL_END)

# Initialize GroupSyncRead instances
groupSyncRead = GroupSyncRead(portHandler, packetHandler, ADDR_STS_PRESENT_POSITION, PRESENT_POSITION_SIZE)
groupSyncReadTorque = GroupSyncRead(portHandler, packetHandler, ADDR_STS_TORQUE_ENABLE, TORQUE_ENABLE_SIZE)

# Open port
if portHandler.openPort():
    print(f"Succeeded to open the port {args.port}")
else:
    print(f"Failed to open the port {args.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()

# Add parameter storage for servo IDs 1-n
for servoId in range(1, args.n + 1):
    servo_addparam_result = groupSyncRead.addParam(servoId)
    torque_addparam_result = groupSyncReadTorque.addParam(servoId)
    if servo_addparam_result != True or torque_addparam_result != True:
        print(f"[ID:{servoId:03d}] groupSyncRead addparam failed")
        quit()

while True:
    print("Press any key to read position! (or press ESC to quit)")
    if getch() == chr(0x1b):
        break

# Syncread present position and torque status
    servo_comm_result = groupSyncRead.txRxPacket()
    torque_comm_result = groupSyncReadTorque.txRxPacket()
    if servo_comm_result != COMM_SUCCESS or torque_comm_result != COMM_SUCCESS:
        print(f"Failed to sync read: {packetHandler.getTxRxResult(servo_comm_result)}")
        continue
        
    # Display position for each servo
    print("\nServo Positions:")
    print("-----------------")
    
    for servoId in range(1, args.n + 1):
        # Check if data from the servo is available
        if groupSyncRead.isAvailable(servoId, ADDR_STS_PRESENT_POSITION, PRESENT_POSITION_SIZE):
            # Get the position value (4 bytes data: Position + Speed)
            position_speed_data = groupSyncRead.getData(servoId, ADDR_STS_PRESENT_POSITION, PRESENT_POSITION_SIZE)
            
            # Extract actual position (lower 2 bytes)
            position = SCS_LOWORD(position_speed_data)
            
            # Extract speed (higher 2 bytes)
            speed = SCS_HIWORD(position_speed_data)
            
            # Get torque status
            torque_status = groupSyncReadTorque.getData(servoId, ADDR_STS_TORQUE_ENABLE, TORQUE_ENABLE_SIZE)
            torque_enabled = "ON" if torque_status else "OFF"
            print(f"Servo ID: {servoId} | Position: {position} | Speed: {SCS_TOHOST(speed, 15)} | Torque: {torque_enabled}")
        else:
            print(f"Servo ID: {servoId} | Failed to get data")
    
    print("-----------------\n")

# Clear parameter storage before exit
groupSyncRead.clearParam()

# Close port
portHandler.closePort()

```

FTServo/FTServo_Python

内容风险标识

评论 (0)

FTServo/FTServo_Python .gitee-modal { width: 500px !important; }

内容风险标识