when'd i last commit this pos?
This commit is contained in:
Thomas Ales [M S E]
@@ -0,0 +1,172 @@
|
||||
"""
|
||||
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())
|
||||
Reference in New Issue
Block a user