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