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