The S-Curve

from hub import port, sound, light_matrix
import runloop, motor, motor_pair, distance_sensor, color_sensor, color

DRIVE      = motor_pair.PAIR_1
STEER      = port.D            # large motor, steers the back
TURRET     = port.C            # large motor that swings the distance sensor
EYES       = port.B            # distance sensor (rides on the turret)
BELLY      = port.F            # color sensor pointing down
STOP_MM    = 200               # wall closer than 20 cm ahead = stop and look
OPEN_MM    = 300               # farther than this = OPEN (about one lane width - measure yours!)
LOOK_RIGHT = 90                # turret position that looks right (tune for your gearing)
LOOK_LEFT  = -90               # turret position that looks left
DEG_PER_CM = 20                # wheel degrees per cm: tune
TURN_CM    = 20                # roll while steering: tune

def wall_closer_than(mm):
    d = distance_sensor.distance(EYES)
    return 0 < d < mm

def on_color(c):
    return color_sensor.color(BELLY) == c

async def drive_cm(cm, speed=300):
    await motor_pair.move_for_degrees(DRIVE, int(cm * DEG_PER_CM), 0, velocity=speed)

async def turn_right():
    await motor.run_to_absolute_position(STEER, 60, 500)
    await drive_cm(TURN_CM)
    await motor.run_to_absolute_position(STEER, 0, 500)

async def turn_left():
    await motor.run_to_absolute_position(STEER, -60, 500)
    await drive_cm(TURN_CM)
    await motor.run_to_absolute_position(STEER, 0, 500)

async def turn_around():
    await turn_left()          # two 90-degree turns = a U-turn inside the lane
    await turn_left()

async def celebrate():
    light_matrix.show_image(light_matrix.IMAGE_HAPPY)
    await sound.beep(880, 300)
    await sound.beep(1175, 500)

async def look_open(angle):
    """Swing the turret to 'angle' and say whether that way is OPEN."""
    await motor.run_to_absolute_position(TURRET, angle, 700)
    await runloop.sleep_ms(200)                 # let the sensor settle
    d = distance_sensor.distance(EYES)          # -1 = nothing seen = wide open
    return d == -1 or d > OPEN_MM

async def look_around():
    right_open = await look_open(LOOK_RIGHT)
    left_open = await look_open(LOOK_LEFT)
    await motor.run_to_absolute_position(TURRET, 0, 700)   # face ahead again
    return right_open, left_open

async def drive_until_wall_or(stop_color):
    """Drive until a wall is close ahead, or stop_color is under the car."""
    motor_pair.move(DRIVE, 0, velocity=300)
    await runloop.until(lambda: wall_closer_than(STOP_MM) or on_color(stop_color))
    motor_pair.stop(DRIVE)

async def hug_right_wall_until(stop_color):
    """Stop-and-look maze solver (right-hand rule). Returns (route, dead_ends)."""
    route = []
    dead_ends = 0
    while not on_color(stop_color):
        await drive_until_wall_or(stop_color)
        if on_color(stop_color):
            break
        right_open, left_open = await look_around()
        if right_open:
            await turn_right()
            route.append("R")
        elif left_open:
            await turn_left()
            route.append("L")
        else:                               # all sides blocked: a dead end
            await turn_around()
            dead_ends += 1
            route.append("U")
    return route, dead_ends

async def main():
    motor_pair.pair(DRIVE, port.A, port.E)      # your drive motor ports
    await hug_right_wall_until(color.GREEN)
    await celebrate()

runloop.run(main())