Thomas Ales [M S E]
301 lines
9.5 KiB
Python
301 lines
9.5 KiB
Python
"""
|
|
BBD202 Snake Scan Test - 20x20mm Area
|
|
|
|
Scans a 20mm x 20mm area centered at X=55, Y=37.5 in a snake pattern.
|
|
Row spacing: 0.5mm
|
|
Movement order: X+, Y+, X-, Y+ (snake pattern)
|
|
"""
|
|
|
|
import sys
|
|
import time
|
|
import argparse
|
|
from hardware.bbd202 import MotionController, MotorStatusBits
|
|
|
|
|
|
# Scan parameters
|
|
CENTER_X = 55.0 # mm
|
|
CENTER_Y = 37.5 # mm
|
|
SCAN_WIDTH = 20.0 # mm (X direction)
|
|
SCAN_HEIGHT = 20.0 # mm (Y direction)
|
|
ROW_SPACING = 0.5 # mm (Y step between rows)
|
|
|
|
# Calculated bounds
|
|
START_X = CENTER_X - SCAN_WIDTH / 2 # 45.0 mm
|
|
END_X = CENTER_X + SCAN_WIDTH / 2 # 65.0 mm
|
|
START_Y = CENTER_Y - SCAN_HEIGHT / 2 # 27.5 mm
|
|
END_Y = CENTER_Y + SCAN_HEIGHT / 2 # 47.5 mm
|
|
|
|
|
|
def snake_scan(mc: MotionController) -> bool:
|
|
"""
|
|
Execute a snake scan pattern over the 20x20mm area.
|
|
|
|
Pattern (X+, Y+, X-, Y+):
|
|
Row 0: X=45 -> X=65 (X+)
|
|
Step: Y += 0.5 (Y+)
|
|
Row 1: X=65 -> X=45 (X-)
|
|
Step: Y += 0.5 (Y+)
|
|
... repeat until Y reaches 47.5mm
|
|
"""
|
|
print("\n" + "=" * 70)
|
|
print("SNAKE SCAN TEST: 20mm x 20mm")
|
|
print("=" * 70)
|
|
print(f"Center: X={CENTER_X:.1f}mm, Y={CENTER_Y:.1f}mm")
|
|
print(f"Bounds: X=[{START_X:.1f}, {END_X:.1f}]mm, Y=[{START_Y:.1f}, {END_Y:.1f}]mm")
|
|
print(f"Row spacing: {ROW_SPACING}mm")
|
|
print(f"Pattern: X+, Y+, X-, Y+ (snake)")
|
|
|
|
# Calculate number of rows
|
|
num_rows = int(SCAN_HEIGHT / ROW_SPACING) + 1
|
|
print(f"Total rows: {num_rows}")
|
|
|
|
# Stop automatic ACK to prevent interference with move commands
|
|
# We'll send manual ACKs after each move completes
|
|
mc.stop_status_ack()
|
|
print("Using manual ACK mode for scan")
|
|
|
|
# First, move to the starting position
|
|
print(f"\n--- Moving to start position ({START_X:.1f}, {START_Y:.1f}) ---")
|
|
mc.move_to_fast(x=START_X, y=START_Y)
|
|
|
|
move_start = time.time()
|
|
while not mc.poll_until_idle(tolerance=0.005, timeout=0.05):
|
|
if time.time() - move_start > 30.0:
|
|
print("** TIMEOUT moving to start position!")
|
|
return False
|
|
|
|
# Verify starting position
|
|
mc.poll_positions()
|
|
time.sleep(0.1)
|
|
curr_x = mc.position_x
|
|
curr_y = mc.position_y
|
|
print(f"At start: X={curr_x:.4f}mm, Y={curr_y:.4f}mm")
|
|
|
|
# Generate snake path waypoints
|
|
waypoints = []
|
|
for row in range(num_rows):
|
|
y_pos = START_Y + row * ROW_SPACING
|
|
|
|
if row % 2 == 0:
|
|
# Even row: move X+ (left to right)
|
|
waypoints.append((END_X, y_pos, f"Row {row + 1}/{num_rows} X+ (Y={y_pos:.1f})"))
|
|
else:
|
|
# Odd row: move X- (right to left)
|
|
waypoints.append((START_X, y_pos, f"Row {row + 1}/{num_rows} X- (Y={y_pos:.1f})"))
|
|
|
|
print(f"\nTotal waypoints: {len(waypoints)}")
|
|
print("-" * 70)
|
|
|
|
all_passed = True
|
|
total_distance = 0.0
|
|
total_time = 0.0
|
|
scan_start_time = time.time()
|
|
|
|
# Execute the snake pattern
|
|
for i, (target_x, target_y, description) in enumerate(waypoints):
|
|
# Record start position for distance calculation
|
|
mc.poll_positions()
|
|
time.sleep(0.02)
|
|
prev_x = mc.position_x
|
|
prev_y = mc.position_y
|
|
|
|
if prev_x is not None and prev_y is not None:
|
|
distance = ((target_x - prev_x) ** 2 + (target_y - prev_y) ** 2) ** 0.5
|
|
total_distance += distance
|
|
|
|
# Clear any previous error
|
|
mc.clear_last_error()
|
|
|
|
# Issue move command
|
|
move_start = time.time()
|
|
mc.move_to_fast(x=target_x, y=target_y)
|
|
|
|
# Wait for move to complete
|
|
while not mc.poll_until_idle(tolerance=0.005, timeout=0.05):
|
|
elapsed = time.time() - move_start
|
|
|
|
# Timeout check
|
|
if elapsed > 30.0:
|
|
print(f"\n** TIMEOUT on {description} after 30s!")
|
|
all_passed = False
|
|
mc.clear_pending_moves()
|
|
break
|
|
|
|
# Move complete - send ACK to keep controller responsive
|
|
mc.ack_status_update()
|
|
|
|
elapsed = time.time() - move_start
|
|
total_time += elapsed
|
|
|
|
mc.poll_positions()
|
|
time.sleep(0.02)
|
|
final_x = mc.position_x
|
|
final_y = mc.position_y
|
|
|
|
if final_x is not None and final_y is not None:
|
|
x_err = final_x - target_x
|
|
y_err = final_y - target_y
|
|
total_err = (x_err ** 2 + y_err ** 2) ** 0.5
|
|
|
|
# Print progress (compact format)
|
|
status = "OK" if total_err < 0.05 else "ERR"
|
|
print(f" {description:35} -> X={final_x:7.3f} Y={final_y:7.3f} err={total_err * 1000:5.1f}um [{status}]")
|
|
|
|
if total_err > 0.05: # 50um tolerance
|
|
all_passed = False
|
|
|
|
# Check for errors after move
|
|
err = mc.check_for_errors()
|
|
if err:
|
|
print(f" ** Error: {err}")
|
|
all_passed = False
|
|
|
|
last_err = mc.last_error
|
|
if last_err:
|
|
print(f" ** Last error: {last_err}")
|
|
all_passed = False
|
|
|
|
scan_elapsed = time.time() - scan_start_time
|
|
|
|
# Return to start position
|
|
print(f"\n--- Returning to start position ({START_X:.1f}, {START_Y:.1f}) ---")
|
|
move_start = time.time()
|
|
mc.move_to_fast(x=START_X, y=START_Y)
|
|
|
|
while not mc.poll_until_idle(tolerance=0.005, timeout=0.05):
|
|
if time.time() - move_start > 30.0:
|
|
print("** TIMEOUT returning to start")
|
|
mc.clear_pending_moves()
|
|
break
|
|
|
|
# Final summary
|
|
print(f"\n{'=' * 70}")
|
|
print("SNAKE SCAN SUMMARY")
|
|
print(f"{'=' * 70}")
|
|
|
|
mc.poll_positions()
|
|
time.sleep(0.1)
|
|
final_x = mc.position_x
|
|
final_y = mc.position_y
|
|
|
|
if final_x is not None and final_y is not None:
|
|
return_x_err = abs(final_x - START_X)
|
|
return_y_err = abs(final_y - START_Y)
|
|
return_err = (return_x_err ** 2 + return_y_err ** 2) ** 0.5
|
|
|
|
print(f"Scan area: {SCAN_WIDTH:.1f}mm x {SCAN_HEIGHT:.1f}mm")
|
|
print(f"Center: X={CENTER_X:.1f}mm, Y={CENTER_Y:.1f}mm")
|
|
print(f"Row spacing: {ROW_SPACING}mm")
|
|
print(f"Rows completed: {num_rows}")
|
|
print(f"Total distance: {total_distance:.2f}mm")
|
|
print(f"Scan time: {scan_elapsed:.2f}s")
|
|
print(f"Average speed: {total_distance / scan_elapsed:.2f}mm/s")
|
|
print(f"Return error: {return_err * 1000:.1f}um (X={return_x_err * 1000:.1f}um, Y={return_y_err * 1000:.1f}um)")
|
|
|
|
if return_err > 0.05:
|
|
print(f"\n** FAIL: Return position error exceeds 50um")
|
|
all_passed = False
|
|
|
|
# Restart automatic ACK
|
|
mc.start_status_ack()
|
|
|
|
if all_passed:
|
|
print(f"\n** PASS: Snake scan completed successfully **")
|
|
else:
|
|
print(f"\n** FAIL: Issues detected during snake scan **")
|
|
|
|
return all_passed
|
|
|
|
|
|
def main():
|
|
parser = argparse.ArgumentParser(description="BBD202 Snake Scan Test - 20x20mm Area")
|
|
parser.add_argument("--auto-home", action="store_true",
|
|
help="Automatically home axes if not homed (no prompt)")
|
|
args = parser.parse_args()
|
|
|
|
print("=" * 70)
|
|
print("BBD202 Snake Scan Test - 20x20mm Area")
|
|
print("=" * 70)
|
|
|
|
mc = None
|
|
try:
|
|
# Connect to controller
|
|
print("\nConnecting to BBD202 controller...")
|
|
mc = MotionController()
|
|
mc.connect(enable_updates=True)
|
|
print("Connected!")
|
|
|
|
# Brief hardware info
|
|
try:
|
|
hw_info = mc.get_hw_info()
|
|
print(f"\nHardware: {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}")
|
|
|
|
# Check homing status
|
|
print("\n--- Checking Homing Status ---")
|
|
mc.request_status_update(mc.DEST_X_AXIS)
|
|
mc.request_status_update(mc.DEST_Y_AXIS)
|
|
time.sleep(0.2)
|
|
mc.poll_status()
|
|
time.sleep(0.1)
|
|
|
|
x_homed = mc.is_homed_x
|
|
y_homed = mc.is_homed_y
|
|
|
|
print(f"X-axis homed: {x_homed}")
|
|
print(f"Y-axis homed: {y_homed}")
|
|
|
|
if not (x_homed and y_homed):
|
|
print("\n** Axes not homed! **")
|
|
|
|
# Check if we should auto-home or prompt
|
|
should_home = args.auto_home
|
|
if not should_home:
|
|
try:
|
|
response = input("Home axes now? (y/n): ").strip().lower()
|
|
should_home = response == 'y'
|
|
except EOFError:
|
|
print("Non-interactive mode detected. Use --auto-home flag.")
|
|
return 1
|
|
|
|
if should_home:
|
|
print("\nHoming X-axis...")
|
|
x_result = mc.home_x_axis(timeout=30.0)
|
|
print(f"X-axis: {'SUCCESS' if x_result else 'FAILED'}")
|
|
|
|
print("\nHoming Y-axis...")
|
|
y_result = mc.home_y_axis(timeout=30.0)
|
|
print(f"Y-axis: {'SUCCESS' if y_result else 'FAILED'}")
|
|
|
|
if not (x_result and y_result):
|
|
print("\n** Homing failed. Cannot continue. **")
|
|
return 1
|
|
else:
|
|
print("\nCannot run snake scan without homing. Exiting.")
|
|
return 1
|
|
|
|
# Run the snake scan
|
|
success = snake_scan(mc)
|
|
|
|
return 0 if success else 1
|
|
|
|
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())
|