Thomas Ales [M S E]
173 lines
6.3 KiB
Python
173 lines
6.3 KiB
Python
"""
|
|
Test script to examine raw status update packets from the BBD202 controller.
|
|
|
|
This script:
|
|
1. Connects to the controller at a low level
|
|
2. Sends HW_START_UPDATEMSGS to enable automatic status updates
|
|
3. Captures and displays raw packets to verify the format matches our parsing
|
|
"""
|
|
|
|
import sys
|
|
import time
|
|
import struct
|
|
from hardware.bbd202 import MotionController, MsgId
|
|
|
|
|
|
def hexdump(data: bytes, prefix: str = "") -> str:
|
|
"""Format bytes as hex dump."""
|
|
hex_str = " ".join(f"{b:02X}" for b in data)
|
|
ascii_str = "".join(chr(b) if 32 <= b < 127 else "." for b in data)
|
|
return f"{prefix}{hex_str} |{ascii_str}|"
|
|
|
|
|
|
def decode_status_update(data: bytes) -> dict:
|
|
"""Decode a MOT_GET_USTATUSUPDATE message data payload."""
|
|
if len(data) < 14:
|
|
return {"error": f"Data too short: {len(data)} bytes, expected 14"}
|
|
|
|
chan_ident = struct.unpack('<H', data[0:2])[0]
|
|
position_counts = struct.unpack('<i', data[2:6])[0]
|
|
velocity_counts = struct.unpack('<H', data[6:8])[0]
|
|
motor_current = struct.unpack('<H', data[8:10])[0]
|
|
status_bits = struct.unpack('<I', data[10:14])[0]
|
|
|
|
# Convert to physical units
|
|
ENCODER_COUNTS_PER_MM = 20000
|
|
position_mm = position_counts / ENCODER_COUNTS_PER_MM
|
|
|
|
return {
|
|
"chan_ident": chan_ident,
|
|
"position_counts": position_counts,
|
|
"position_mm": position_mm,
|
|
"velocity_counts": velocity_counts,
|
|
"motor_current": motor_current,
|
|
"status_bits": status_bits,
|
|
"status_hex": f"0x{status_bits:08X}"
|
|
}
|
|
|
|
|
|
def main():
|
|
print("=" * 70)
|
|
print("BBD202 Status Update Packet Test")
|
|
print("=" * 70)
|
|
|
|
mc = None
|
|
try:
|
|
# Connect to controller WITHOUT enabling updates yet
|
|
print("\nConnecting to BBD202 controller...")
|
|
mc = MotionController()
|
|
mc.connect(enable_updates=False) # Don't auto-enable updates
|
|
print("Connected!")
|
|
|
|
# Get hardware info
|
|
try:
|
|
hw_info = mc.get_hw_info()
|
|
print(f"Hardware: {hw_info['model']} (S/N: {hw_info['serial_number']})")
|
|
print(f"Firmware: {hw_info['firmware_version']}")
|
|
except Exception as e:
|
|
print(f"Could not get hardware info: {e}")
|
|
|
|
# Clear any pending messages
|
|
print("\nClearing RX queue...")
|
|
msgs = mc.get_all_messages()
|
|
print(f"Cleared {len(msgs)} pending messages")
|
|
|
|
# Now enable status updates
|
|
print("\n" + "-" * 70)
|
|
print("Sending HW_START_UPDATEMSGS to enable automatic status updates...")
|
|
print("-" * 70)
|
|
mc.start_update_messages()
|
|
|
|
# Wait a moment for updates to start arriving
|
|
time.sleep(0.5)
|
|
|
|
# Collect messages for a few seconds
|
|
print("\nCollecting status update packets for 3 seconds...")
|
|
print("(Looking for MOT_GET_USTATUSUPDATE = 0x0491)")
|
|
print()
|
|
|
|
start_time = time.time()
|
|
update_count = 0
|
|
other_count = 0
|
|
|
|
while time.time() - start_time < 3.0:
|
|
msg = mc.get_message(timeout=0.1)
|
|
if msg:
|
|
elapsed = time.time() - start_time
|
|
|
|
if msg.msg_id == MsgId.MOT_GET_USTATUSUPDATE:
|
|
update_count += 1
|
|
source_name = "X-axis" if msg.source == 0x21 else "Y-axis" if msg.source == 0x22 else f"0x{msg.source:02X}"
|
|
|
|
print(f"[{elapsed:5.2f}s] MOT_GET_USTATUSUPDATE from {source_name}")
|
|
print(f" Raw ({len(msg.raw)} bytes): {hexdump(msg.raw)}")
|
|
print(f" Data ({len(msg.data)} bytes): {hexdump(msg.data)}")
|
|
|
|
decoded = decode_status_update(msg.data)
|
|
if "error" in decoded:
|
|
print(f" DECODE ERROR: {decoded['error']}")
|
|
else:
|
|
print(f" Decoded: chan={decoded['chan_ident']}, "
|
|
f"pos={decoded['position_mm']:.4f}mm ({decoded['position_counts']} counts), "
|
|
f"vel={decoded['velocity_counts']}, cur={decoded['motor_current']}, "
|
|
f"status={decoded['status_hex']}")
|
|
print()
|
|
else:
|
|
other_count += 1
|
|
msg_name = MsgId(msg.msg_id).name if msg.msg_id in [m.value for m in MsgId] else f"0x{msg.msg_id:04X}"
|
|
print(f"[{elapsed:5.2f}s] Other message: {msg_name} from 0x{msg.source:02X}")
|
|
print(f" Raw: {hexdump(msg.raw)}")
|
|
print()
|
|
|
|
print("-" * 70)
|
|
print(f"Summary: Received {update_count} status updates, {other_count} other messages")
|
|
print("-" * 70)
|
|
|
|
if update_count == 0:
|
|
print("\n*** WARNING: No status updates received! ***")
|
|
print("Possible causes:")
|
|
print(" 1. HW_START_UPDATEMSGS not being processed")
|
|
print(" 2. Controller firmware doesn't support automatic updates")
|
|
print(" 3. Updates are being sent but not parsed correctly")
|
|
print("\nTrying to manually request a status update...")
|
|
|
|
# Try requesting status update manually
|
|
mc.request_status_update(mc.DEST_X_AXIS)
|
|
mc.request_status_update(mc.DEST_Y_AXIS)
|
|
time.sleep(0.5)
|
|
|
|
msgs = mc.get_all_messages()
|
|
print(f"\nReceived {len(msgs)} messages after manual request:")
|
|
for msg in msgs:
|
|
msg_name = MsgId(msg.msg_id).name if msg.msg_id in [m.value for m in MsgId] else f"0x{msg.msg_id:04X}"
|
|
print(f" {msg_name} from 0x{msg.source:02X}: {hexdump(msg.raw)}")
|
|
|
|
# Also check cached positions
|
|
print("\n" + "-" * 70)
|
|
print("Cached positions in driver:")
|
|
print("-" * 70)
|
|
print(f" X position: {mc.get_stage_position_x()}")
|
|
print(f" Y position: {mc.get_stage_position_y()}")
|
|
print(f" X encoder: {mc.encoder_count_x}")
|
|
print(f" Y encoder: {mc.encoder_count_y}")
|
|
|
|
return 0
|
|
|
|
except KeyboardInterrupt:
|
|
print("\n\nTest interrupted by user")
|
|
return 1
|
|
except Exception as e:
|
|
print(f"\n*** ERROR: {e}")
|
|
import traceback
|
|
traceback.print_exc()
|
|
return 1
|
|
finally:
|
|
if mc is not None:
|
|
print("\nDisconnecting from controller...")
|
|
mc.disconnect()
|
|
print("Disconnected")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
sys.exit(main())
|