Practical case: UGV line follower with Raspberry Pi

"""
ugv_hardware.py
Hardware Abstraction Layer for 2WD UGV with TB6612FNG and TCRT5000.
Includes mock classes for off-hardware dry-run validation.
"""

import time

try:
    from gpiozero import PWMOutputDevice, DigitalOutputDevice, DigitalInputDevice
    GPIO_AVAILABLE = True
except ImportError:
    GPIO_AVAILABLE = False

class MockMotor:
    """Mock motor class for dry-run validation."""
    def __init__(self, name):
        self.name = name
        self.current_speed = 0.0

    def drive(self, speed):
        # Constrain speed between -1.0 and 1.0
        self.current_speed = max(min(speed, 1.0), -1.0)
        direction = "FORWARD" if self.current_speed > 0 else "REVERSE" if self.current_speed < 0 else "STOPPED"
        print(f"[MOCK] {self.name} Motor -> Speed: {abs(self.current_speed):.2f} | Dir: {direction}")

class MockSensorArray:
    """Mock sensor array that cycles through predefined track states."""
    def __init__(self):
        # (Left, Center, Right) - 1 means line detected, 0 means no line
        self.test_sequence = [
            (0, 1, 0),  # Centered
            (1, 1, 0),  # Drifting slightly right (left sensor hits line)
            (1, 0, 0),  # Drifting hard right
            (0, 1, 0),  # Centered again
            (0, 0, 1),  # Drifting hard left
            (0, 0, 0),  # Line lost
        ]
        self.step = 0

    def read_sensors(self):
        state = self.test_sequence[self.step % len(self.test_sequence)]
        self.step += 1
        print(f"[MOCK] Sensors read (L, C, R): {state}")
        return state

class RealMotor:
    """Real implementation for TB6612FNG motor control using gpiozero."""
    def __init__(self, pwm_pin, in1_pin, in2_pin):
        self.pwm = PWMOutputDevice(pwm_pin)
        self.in1 = DigitalOutputDevice(in1_pin)
        self.in2 = DigitalOutputDevice(in2_pin)

    def drive(self, speed):
        speed = max(min(speed, 1.0), -1.0)
        if speed > 0:
            self.in1.on()
            self.in2.off()
            self.pwm.value = speed
        elif speed < 0:
            self.in1.off()
            self.in2.on()
            self.pwm.value = abs(speed)
        else:
            self.in1.off()
            self.in2.off()
            self.pwm.value = 0.0

class RealSensorArray:
    """Real implementation for TCRT5000 array using gpiozero."""
    def __init__(self, left_pin, center_pin, right_pin):
        self.left = DigitalInputDevice(left_pin)
        self.center = DigitalInputDevice(center_pin)
        self.right = DigitalInputDevice(right_pin)

    def read_sensors(self):
        # Assuming sensor outputs 1 when dark line is detected
        return (self.left.value, self.center.value, self.right.value)

def get_hardware(dry_run=False):
    """Factory function to return hardware interfaces based on execution mode."""
    if dry_run or not GPIO_AVAILABLE:
        if not dry_run:
            print("Warning: gpiozero not found. Defaulting to dry-run mode.")
        left_motor = MockMotor("Left")
        right_motor = MockMotor("Right")
        sensors = MockSensorArray()
        return left_motor, right_motor, sensors

    # Hardware pin mapping based on setup table
    left_motor = RealMotor(pwm_pin=17, in1_pin=27, in2_pin=22)
    right_motor = RealMotor(pwm_pin=18, in1_pin=23, in2_pin=24)
    sensors = RealSensorArray(left_pin=5, center_pin=6, right_pin=13)

    return left_motor, right_motor, sensors

"""
ugv_line_follower.py
Main control loop for the UGV line following prototype.
Reads sensor states and applies differential steering.
"""

import argparse
import time
import sys
from ugv_hardware import get_hardware

# Configuration parameters
BASE_SPEED = 0.5    # Normal forward speed (0.0 to 1.0)
TURN_SPEED = 0.6    # Speed of the outer wheel during a turn
REDUCE_SPEED = 0.2  # Speed of the inner wheel during a turn
LOOP_DELAY = 0.1    # Delay between control loops in seconds

def follow_line(left_motor, right_motor, sensors, max_iterations=None):
    """
    Main control loop. Evaluates sensor states to determine motor speeds.
    """
    print("--- Starting UGV Line Follower ---")
    print("Press Ctrl+C to stop.")

    iterations = 0
    try:
        while True:
            if max_iterations and iterations >= max_iterations:
                print("Max iterations reached. Stopping.")
                break

            left_val, center_val, right_val = sensors.read_sensors()

            # State 1: Centered on the line
            if center_val == 1 and left_val == 0 and right_val == 0:
                left_motor.drive(BASE_SPEED)
                right_motor.drive(BASE_SPEED)

            # State 2: Drifting Right (Left sensor sees line)
            elif left_val == 1 and right_val == 0:
                # Turn Left: Right motor fast, Left motor slow
                left_motor.drive(REDUCE_SPEED)
                right_motor.drive(TURN_SPEED)

            # State 3: Drifting Left (Right sensor sees line)
            elif right_val == 1 and left_val == 0:
                # Turn Right: Left motor fast, Right motor slow
                left_motor.drive(TURN_SPEED)
                right_motor.drive(REDUCE_SPEED)

            # State 4: Intersection or perpendicular line (All sensors see line)
            elif left_val == 1 and center_val == 1 and right_val == 1:
                print("Intersection detected. Stopping for safety.")
                left_motor.drive(0.0)
                right_motor.drive(0.0)
                break

            # State 5: Line completely lost (No sensors see line)
            elif left_val == 0 and center_val == 0 and right_val == 0:
                print("Line lost. Halting.")
                left_motor.drive(0.0)
                right_motor.drive(0.0)
                break

            else:
                # Catch-all for ambiguous states (e.g., 1, 0, 1) - maintain base speed cautiously
                left_motor.drive(BASE_SPEED * 0.5)
                right_motor.drive(BASE_SPEED * 0.5)

            iterations += 1
            time.sleep(LOOP_DELAY)

    except KeyboardInterrupt:
        print("\nManual interrupt received.")
    finally:
        # Failsafe: Ensure motors are stopped on exit
        print("Shutting down motors.")
        left_motor.drive(0.0)
        right_motor.drive(0.0)

def main():
    parser = argparse.ArgumentParser(description="UGV Line Follower Control")
    parser.add_argument("--dry-run", action="store_true", help="Run without hardware using mock interfaces")
    parser.add_argument("--self-test", action="store_true", help="Run a short automated test sequence and exit")
    args = parser.parse_args()

    # Initialize hardware or mocks
    left_motor, right_motor, sensors = get_hardware(dry_run=args.dry_run)

    # Determine execution length
    max_iters = 6 if args.self-test else None

    # Execute control loop
    follow_line(left_motor, right_motor, sensors, max_iterations=max_iters)

if __name__ == "__main__":
    main()