Thomas Ales [M S E]
267 lines
8.4 KiB
Python
267 lines
8.4 KiB
Python
"""
|
|
BBD202 Snake Test
|
|
|
|
Quick test to verify the BBD202 stage driver with a snake scan pattern.
|
|
Scans a 20mm x 20mm area with 2mm row spacing in a back-and-forth pattern.
|
|
"""
|
|
|
|
import sys
|
|
import time
|
|
from hardware.bbd202 import MotionController, MotorStatusBits
|
|
|
|
|
|
def snake_test(mc: MotionController, width: float = 20.0, height: float = 20.0, row_spacing: float = 2.0) -> bool:
|
|
"""
|
|
Execute a snake scan pattern.
|
|
|
|
Args:
|
|
mc: MotionController instance
|
|
width: Scan width in mm (X direction)
|
|
height: Scan height in mm (Y direction)
|
|
row_spacing: Distance between rows in mm
|
|
|
|
Pattern:
|
|
Start (0,0) -> Move +X to (width, 0) -> Move +Y by row_spacing ->
|
|
Move -X to (0, row_spacing) -> Move +Y by row_spacing ->
|
|
... repeat until height is covered
|
|
"""
|
|
print("\n" + "="*70)
|
|
print(f"SNAKE TEST: {width}mm x {height}mm area, {row_spacing}mm row spacing")
|
|
print("="*70)
|
|
|
|
# Get starting position
|
|
mc.poll_positions()
|
|
time.sleep(0.1)
|
|
start_x = mc.position_x
|
|
start_y = mc.position_y
|
|
|
|
if start_x is None or start_y is None:
|
|
print("ERROR: Could not get initial position")
|
|
return False
|
|
|
|
print(f"\nStarting position: X={start_x:.4f}mm, Y={start_y:.4f}mm")
|
|
|
|
# Calculate number of rows
|
|
num_rows = int(height / row_spacing) + 1
|
|
print(f"Number of rows: {num_rows}")
|
|
|
|
# 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 left to right
|
|
waypoints.append((start_x + width, y_pos, f"Row {row+1}/{num_rows} (+X)"))
|
|
else:
|
|
# Odd row: move right to left
|
|
waypoints.append((start_x, y_pos, f"Row {row+1}/{num_rows} (-X)"))
|
|
|
|
print(f"Total waypoints: {len(waypoints)}")
|
|
|
|
all_passed = True
|
|
total_distance = 0.0
|
|
total_time = 0.0
|
|
|
|
# Execute the snake pattern
|
|
for i, (target_x, target_y, description) in enumerate(waypoints):
|
|
print(f"\n--- Move {i+1}/{len(waypoints)}: {description} ---")
|
|
print(f"Target: X={target_x:.4f}mm, Y={target_y:.4f}mm")
|
|
|
|
# Record start time and position
|
|
move_start = time.time()
|
|
mc.poll_positions()
|
|
time.sleep(0.02)
|
|
curr_x = mc.position_x
|
|
curr_y = mc.position_y
|
|
|
|
if curr_x is not None and curr_y is not None:
|
|
distance = ((target_x - curr_x)**2 + (target_y - curr_y)**2)**0.5
|
|
total_distance += distance
|
|
|
|
# Clear any previous error
|
|
mc.clear_last_error()
|
|
|
|
# Issue move command
|
|
mc.move_to_fast(x=target_x, y=target_y)
|
|
|
|
# Wait for move to complete with periodic progress updates
|
|
sample_count = 0
|
|
max_err = 0.0
|
|
|
|
while not mc.poll_until_idle(tolerance=0.005, timeout=0.05):
|
|
elapsed = time.time() - move_start
|
|
curr_x = mc.position_x
|
|
curr_y = mc.position_y
|
|
|
|
if curr_x is not None and curr_y is not None:
|
|
x_err = curr_x - target_x
|
|
y_err = curr_y - target_y
|
|
total_err = (x_err**2 + y_err**2)**0.5
|
|
max_err = max(max_err, total_err)
|
|
|
|
# Print every 10th sample
|
|
if sample_count % 10 == 0:
|
|
print(f" {elapsed:>5.2f}s X={curr_x:>8.4f} Y={curr_y:>8.4f} err={total_err*1000:>6.1f}um", end='\r')
|
|
|
|
sample_count += 1
|
|
|
|
# Timeout check
|
|
if elapsed > 30.0:
|
|
print(f"\n** TIMEOUT after 30s - stage may be hung!")
|
|
all_passed = False
|
|
mc.clear_pending_moves()
|
|
break
|
|
|
|
# Move complete
|
|
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(f" {elapsed:>5.2f}s X={final_x:>8.4f} Y={final_y:>8.4f} err={total_err*1000:>6.1f}um ** DONE **")
|
|
|
|
if total_err > 0.05: # 50um tolerance
|
|
print(f" ** WARNING: Position error exceeds 50um: {total_err*1000:.1f}um")
|
|
all_passed = False
|
|
|
|
# Check for errors after move
|
|
err = mc.check_for_errors()
|
|
if err:
|
|
print(f" ** Error after move: {err}")
|
|
all_passed = False
|
|
|
|
last_err = mc.last_error
|
|
if last_err:
|
|
print(f" ** Last error: {last_err}")
|
|
all_passed = False
|
|
|
|
# Return to start position
|
|
print(f"\n--- Returning to start position ---")
|
|
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
|
|
|
|
elapsed = time.time() - move_start
|
|
total_time += elapsed
|
|
|
|
# Final summary
|
|
print(f"\n{'='*70}")
|
|
print("SNAKE TEST 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"Start position: X={start_x:.4f}mm, Y={start_y:.4f}mm")
|
|
print(f"Final position: X={final_x:.4f}mm, Y={final_y:.4f}mm")
|
|
print(f"Return error: {return_err*1000:.1f}um (X={return_x_err*1000:.1f}um, Y={return_y_err*1000:.1f}um)")
|
|
print(f"Total distance: {total_distance:.2f}mm")
|
|
print(f"Total time: {total_time:.2f}s")
|
|
print(f"Average speed: {total_distance/total_time:.2f}mm/s")
|
|
print(f"Waypoints: {len(waypoints)}")
|
|
|
|
if return_err > 0.05: # 50um tolerance
|
|
print(f"\n** FAIL: Did not return to start position (error: {return_err*1000:.1f}um > 50um)")
|
|
all_passed = False
|
|
|
|
if all_passed:
|
|
print(f"\n** PASS: Snake test completed successfully **")
|
|
else:
|
|
print(f"\n** FAIL: Issues detected during snake test **")
|
|
|
|
return all_passed
|
|
|
|
|
|
def main():
|
|
print("="*70)
|
|
print("BBD202 Snake Test")
|
|
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 - will home now **")
|
|
|
|
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
|
|
|
|
# Run the snake test
|
|
success = snake_test(mc, width=20.0, height=20.0, row_spacing=2.0)
|
|
|
|
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())
|