run_track.py

#!/usr/bin/env python3
# run_track.py - Robot path following using generated path data
"""
Robot path follower that executes the autogenerated path produced by
path_planner.py (path.csv + checkpoints_cm.csv).  The plan is derived from
actual arena coordinates, so movement always matches the geometry tagged in
fruit_ui.py or measure_arena.py.

The path segments come from path.csv (columns: turn_deg, distance_cm).  If the
files are missing or empty we regenerate them on the fly.
"""
import csv
import time
import sys
import os

# Import the movement control system
try:
    from move_control import RobotController
    import advanced
    from path_planner import build_auto_path
except ImportError as e:
    print(f"ERROR: Required modules not found: {e}")
    print("Make sure move_control.py, advanced.py, and path_planner.py are in the same directory.")
    sys.exit(1)


class PathFollower:
    def __init__(self, script_dir=None):
        self.script_dir = script_dir or os.path.dirname(__file__)
        self.path_segments = []
        self.checkpoints = []
        self.robot = RobotController()
        self._load_plan()

    def _plan_files(self):
        path_csv = os.path.join(self.script_dir, "path.csv")
        checkpoints_csv = os.path.join(self.script_dir, "checkpoints_cm.csv")
        return path_csv, checkpoints_csv

    def _read_path_csv(self, path_csv):
        segments = []
        try:
            with open(path_csv, "r", newline="") as f:
                reader = csv.DictReader(f)
                for row in reader:
                    turn = float(row.get("turn_deg", 0.0))
                    dist = float(row.get("distance_cm", 0.0))
                    segments.append((turn, dist))
        except FileNotFoundError:
            return []
        return segments

    def _read_checkpoints_csv(self, checkpoints_csv):
        checkpoints = []
        try:
            with open(checkpoints_csv, "r", newline="") as f:
                reader = csv.DictReader(f)
                for row in reader:
                    x = float(row.get("x_cm", 0.0))
                    y = float(row.get("y_cm", 0.0))
                    checkpoints.append((x, y))
        except FileNotFoundError:
            return []
        return checkpoints

    def _load_plan(self):
        """Load path segments/checkpoints, regenerating if missing."""
        path_csv, checkpoints_csv = self._plan_files()

        segments = self._read_path_csv(path_csv)
        checkpoints = self._read_checkpoints_csv(checkpoints_csv)

        if not segments or not checkpoints:
            print("Path files missing or empty – regenerating via path_planner.build_auto_path().")
            checkpoints, segments = build_auto_path(self.script_dir)
        if not segments:
            print("ERROR: Unable to build path segments. Ensure fruits/checkpoints are configured.")
            sys.exit(1)

        self.path_segments = segments
        self.checkpoints = checkpoints
        print(f"Loaded {len(self.path_segments)} path segments from path.csv")
    
    def execute_track(self, start_delay=3.0):
        """Execute the complete track using pure encoder-based movement control (NO GYRO)"""
        if not self.path_segments:
            print("No path data to execute")
            return False
            
        print(f"Executing path with {len(self.path_segments)} segments")
        print(f"Starting in {start_delay} seconds...")
        
        # Countdown
        for i in range(int(start_delay), 0, -1):
            print(f"{i}...")
            time.sleep(1.0)
        print("GO!")
        
        success_count = 0
        for i, (angle, measure) in enumerate(self.path_segments):
            print(f"\n--- Segment {i+1}/{len(self.path_segments)} ---")
            print(f"Target: {angle:.1f}° turn, {measure:.1f} cm forward")
            
            # Break down large turns into smaller ones
            if abs(angle) > 135:  # If turn is close to 180 degrees
                print(f"Breaking down {angle:.1f}° turn into two 90° turns")
                
                # First 90-degree turn
                first_turn = 90 if angle > 0 else -90
                print(f"  First turn: {first_turn:.1f}°")
                if not self.robot.turn_to_angle(first_turn):
                    print(f"Failed first turn in segment {i+1}")
                    break
                
                # Second 90-degree turn
                second_turn = angle - first_turn
                print(f"  Second turn: {second_turn:.1f}°")
                if not self.robot.turn_to_angle(second_turn):
                    print(f"Failed second turn in segment {i+1}")
                    break
                
                # Move forward
                if measure > 0:
                    print(f"  Moving forward: {measure:.1f} cm")
                    if not self.robot.move_distance(measure):
                        print(f"Failed forward movement in segment {i+1}")
                        break
            else:
                # Use the precise movement control system for normal turns
                if not self.robot.execute_command(angle, measure):
                    print(f"Failed to execute segment {i+1}")
                    break
                
            success_count += 1
            print(f"Segment {i+1} completed successfully")
        
        print(f"\nPath execution complete: {success_count}/{len(self.path_segments)} segments successful")
        return success_count == len(self.path_segments)
    
    def preview_track(self):
        """Preview the track without executing"""
        if not self.path_segments:
            print("No path data to preview")
            return
            
        print(f"Path Preview ({len(self.path_segments)} segments):")
        print("Segment | Turn Angle | Distance | Notes")
        print("--------|------------|----------|------------------")
        
        cumulative_angle = 0.0
        total_distance = 0.0
        breakdown_count = 0
        
        for i, (angle, measure) in enumerate(self.path_segments):
            cumulative_angle += angle
            total_distance += measure
            
            # Check if this turn will be broken down
            notes = ""
            if abs(angle) > 135:
                notes = "→ 2×90° turns"
                breakdown_count += 1
            
            print(f"   {i+1:2d}   |   {angle:6.1f}°  |  {measure:6.1f} cm | {notes}")
        
        print("--------|------------|----------|------------------")
        print(f" Total  |   {cumulative_angle:6.1f}°  |  {total_distance:6.1f} cm | {breakdown_count} segments split")


def main():
    print("=== Robot Track Follower ===")
    
    # Initialize robot control
    if not advanced.init_bot_control(verbose_telemetry=False):
        print("ERROR: Failed to initialize robot control")
        return
    
    # Create path follower
    follower = PathFollower()
    
    # Preview track
    follower.preview_track()
    
    # Ask user for confirmation
    print("\nOptions:")
    print("1. Execute track")
    print("2. Preview only (exit)")
    print("3. Exit")
    
    try:
        choice = input("\nEnter choice (1-3): ").strip()
        
        if choice == "1":
            print("\nStarting track execution...")
            success = follower.execute_track()
            if success:
                print("Track completed successfully!")
            else:
                print("Track execution failed or incomplete.")
        elif choice == "2":
            print("Preview complete.")
        else:
            print("Exiting.")
            
    except KeyboardInterrupt:
        print("\nInterrupted by user")
    except Exception as e:
        print(f"Error: {e}")
    finally:
        advanced.cleanup()
        print("Robot control cleaned up")


if __name__ == "__main__":
    main()