Compare commits
2 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| e06737fcd1 | |||
| 82a2c26bee |
+181
@@ -0,0 +1,181 @@
|
||||
import requests
|
||||
import serial
|
||||
import time
|
||||
import sys
|
||||
import re
|
||||
|
||||
# ================= CONFIGURATION =================
|
||||
CAMERA_IP = "192.168.0.10"
|
||||
ARDUINO_PORT = "/dev/ttyUSB0"
|
||||
BAUD_RATE = 9600
|
||||
|
||||
# Motion Control
|
||||
ROTATION_SPEED_DPS = 45.0
|
||||
ROTATION_STEP_SIZE = 15.0
|
||||
TOTAL_TARGET_DEGREES = 360.0
|
||||
|
||||
# Timing
|
||||
PHOTO_TO_MOVE_DELAY = 8.0 # Wait after photo before rotation starts
|
||||
SERIAL_TIMEOUT = 10.0 # Increased timeout for long moves
|
||||
# ================================================
|
||||
|
||||
def emergency_shutdown(error_msg, ser=None):
|
||||
"""
|
||||
Failsafe routine: Attempts to switch camera to play mode and exits.
|
||||
"""
|
||||
print(f"\n[!] ERROR DETECTED: {error_msg}")
|
||||
print("Initiating failsafe: Switching camera back to default state...")
|
||||
|
||||
try:
|
||||
requests.get(f"http://{CAMERA_IP}/switch_cammode.cgi?mode=play", timeout=5)
|
||||
print("Camera switched to play mode.")
|
||||
except Exception as e:
|
||||
print(f"Failed to switch camera mode during shutdown: {e}")
|
||||
|
||||
if ser:
|
||||
try:
|
||||
ser.close()
|
||||
print("Serial connection closed.")
|
||||
except:
|
||||
pass
|
||||
|
||||
print("System halted.")
|
||||
sys.exit(1)
|
||||
|
||||
def send_arduino_command(ser, command):
|
||||
"""
|
||||
Sends a command to the Arduino.
|
||||
Does NOT wait for a response here to keep the flow simple,
|
||||
but the response is consumed in the main loop.
|
||||
"""
|
||||
try:
|
||||
ser.write(f"{command}\n".encode())
|
||||
# Small delay to ensure Arduino receives command before next action
|
||||
time.sleep(0.1)
|
||||
except Exception as e:
|
||||
raise Exception(f"Failed to send command '{command}': {e}")
|
||||
|
||||
def wait_for_move_completion(ser, expected_angle, speed_dps):
|
||||
"""
|
||||
Waits until the Arduino reports 'Steps Left: 0'.
|
||||
Falls back to a calculated timeout if serial parsing fails.
|
||||
"""
|
||||
# Calculate expected time for the move as a safety fallback
|
||||
# angle / speed = seconds. Add 10% buffer.
|
||||
expected_time = (expected_angle / speed_dps) * 1.2
|
||||
start_time = time.time()
|
||||
|
||||
print(f"Waiting for movement ({expected_angle}°) to complete...")
|
||||
|
||||
while True:
|
||||
if ser.in_waiting > 0:
|
||||
try:
|
||||
line = ser.readline().decode('utf-8').strip()
|
||||
if not line:
|
||||
continue
|
||||
|
||||
# Check for "Steps Left: 0" in the Arduino status output
|
||||
# Your Arduino prints: " | Steps Left: 0"
|
||||
if "Steps Left: 0" in line:
|
||||
print("Movement complete (Steps Left: 0 detected).")
|
||||
return True
|
||||
|
||||
# Optional: Debug print if needed
|
||||
# print(f"Arduino: {line}")
|
||||
except Exception as e:
|
||||
print(f"Warning: Error reading serial: {e}")
|
||||
|
||||
# Fallback timeout
|
||||
elapsed = time.time() - start_time
|
||||
if elapsed > max(expected_time + 5, 30): # Max 30s or calculated time + 5s
|
||||
print(f"Timeout reached ({elapsed:.1f}s). Assuming move finished.")
|
||||
return False
|
||||
|
||||
time.sleep(0.1) # Prevent CPU spinning
|
||||
|
||||
def main():
|
||||
ser = None
|
||||
try:
|
||||
# 1. Initialize Serial Connection (Persistent)
|
||||
print("Opening serial port...")
|
||||
ser = serial.Serial(ARDUINO_PORT, BAUD_RATE, timeout=1)
|
||||
time.sleep(2) # Wait for Arduino to reset and boot
|
||||
|
||||
# Flush any garbage data from boot
|
||||
ser.reset_input_buffer()
|
||||
ser.reset_output_buffer()
|
||||
|
||||
# 2. Initialize Table Parameters
|
||||
print("Initializing table parameters...")
|
||||
send_arduino_command(ser, f"s{ROTATION_SPEED_DPS}")
|
||||
time.sleep(0.5)
|
||||
send_arduino_command(ser, "m1") # Set to Manual mode (controlled by script)
|
||||
time.sleep(0.5)
|
||||
|
||||
# 3. Prepare Camera State
|
||||
print("Switching camera to shutter mode...")
|
||||
requests.get(f"http://{CAMERA_IP}/switch_cammode.cgi?mode=shutter", timeout=5)
|
||||
print("System ready.")
|
||||
|
||||
current_angle = 0.0
|
||||
total_steps = int(TOTAL_TARGET_DEGREES / ROTATION_STEP_SIZE)
|
||||
|
||||
# Ensure we cover the full circle if 360 is the target
|
||||
# If 360/15 = 24 steps, we do 24 moves.
|
||||
# The loop runs while current_angle < TOTAL (0 to 345), then we do one last check?
|
||||
# Actually, simpler: Loop for exactly the number of steps needed.
|
||||
|
||||
steps_taken = 0
|
||||
while steps_taken <= total_steps:
|
||||
# A. Capture Image
|
||||
angle_label = int(current_angle)
|
||||
print(f"\n--- Step {steps_taken + 1}/{total_steps + 1} ---")
|
||||
print(f"Capturing image at {angle_label}°...")
|
||||
|
||||
try:
|
||||
# 1. Push Shutter (Start exposure)
|
||||
req1 = requests.get(f"http://{CAMERA_IP}/exec_shutter.cgi?type=1st2ndpush", timeout=10)
|
||||
time.sleep(0.1)
|
||||
|
||||
# 2. Release Shutter (End exposure) - CRITICAL STEP
|
||||
# The user specified this command is required to actually take the photo
|
||||
req2 = requests.get(f"http://{CAMERA_IP}/exec_shutter.cgi?com=2nd1strelease", timeout=10)
|
||||
|
||||
if req1.status_code != 200 or req2.status_code != 200:
|
||||
raise Exception(f"Camera returned status {req1.status_code} / {req2.status_code}")
|
||||
|
||||
print("Image captured successfully.")
|
||||
except Exception as e:
|
||||
raise Exception(f"Camera shutter sequence failed: {e}")
|
||||
|
||||
# If this is the last step, we don't need to move or wait
|
||||
if steps_taken == total_steps:
|
||||
print("All steps completed.")
|
||||
break
|
||||
|
||||
# B. Wait for camera processing
|
||||
print(f"Waiting {PHOTO_TO_MOVE_DELAY}s for camera processing...")
|
||||
time.sleep(PHOTO_TO_MOVE_DELAY)
|
||||
|
||||
# C. Rotate Table
|
||||
print(f"Rotating {ROTATION_STEP_SIZE}°...")
|
||||
send_arduino_command(ser, f"a{ROTATION_STEP_SIZE}")
|
||||
|
||||
# D. Wait for movement to finish
|
||||
# We pass the step size and speed to calculate a safe wait time
|
||||
wait_for_move_completion(ser, ROTATION_STEP_SIZE, ROTATION_SPEED_DPS)
|
||||
|
||||
current_angle += ROTATION_STEP_SIZE
|
||||
steps_taken += 1
|
||||
print(f"Progress: {int(current_angle)}/{TOTAL_TARGET_DEGREES} degrees.")
|
||||
|
||||
print("\n[SUCCESS] Full rotation completed successfully.")
|
||||
|
||||
except Exception as e:
|
||||
emergency_shutdown(str(e), ser)
|
||||
finally:
|
||||
if ser and ser.is_open:
|
||||
ser.close()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user