⚡ Fast Solution

"""
NRC 2026 RoboMission Rookie — "Robot Rockstar"
=========================================================================
FAST SOLUTION — uses ONLY the toolkit you already learned.

Same building blocks as simplified.py:  run / stop / reset / PD / move /
turn / follow  + the front & back arms.  Nothing new to learn — the speed
comes from STRATEGY and how the chunks are called, not new functions.

SAME STRUCTURE as the coach, executed faster. The arms hold 3 notes per trip
(stack 2 + push 1) and the colour zones are bottom-left, so the notes MUST be
carried in TWO trips — that part is mandatory, not slow code. The time is won
in HOW each trip is driven:

  1. Find notes by driving STRAIGHT to each one
       no spot-by-spot counting crawl (the coach's slow part) — far fewer
       stop/reset cycles while loading a batch.

  2. Fast empty legs, gentle loaded legs
       LOAD speed (95) when light or returning; CARRY speed (70) when holding
       notes so nothing topples.

  3. Calmer, fewer turns (70) for accuracy.

  4. follow() only where a real line exists — most accurate, zero drift.

  5. The 40 bonus points are free: the route never touches the clef, the
     amplifier or the speakers.

  # CAL  = measure on YOUR robot and the venue mat. Distances are in
  motor-degrees, exactly like move() in simplified.py. SPOT = one
  note-to-note spacing. Starting numbers are scaled from the mat photo.
=========================================================================
"""

from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor
from pybricks.parameters import Port
from pybricks.tools import wait, StopWatch

# ================= HUB & DEVICES =================
hub = PrimeHub()

left_motor = Motor(Port.A)
right_motor = Motor(Port.E)

front = Motor(Port.F)
back = Motor(Port.B)

floor = ColorSensor(Port.C)

timer = StopWatch()

# ================= BASIC SETTINGS =================
kP = 0.6
kD = 8
last_error = 0

# ================= BASIC FUNCTIONS (unchanged toolkit) =================
def run(l, r):
    left_motor.dc(l)
    right_motor.dc(r)

def stop():
    left_motor.brake()
    right_motor.brake()

def reset():
    left_motor.reset_angle(0)
    right_motor.reset_angle(0)

def PD(target, current):
    global last_error
    error = target - current
    output = (error * kP) + ((error - last_error) * kD)
    last_error = error
    return output

# ================= MOVEMENTS (unchanged toolkit) =================
def move(speed, distance):
    reset()
    while (abs(left_motor.angle()) + abs(right_motor.angle())) / 2 < distance:
        run(speed, speed)
    stop()

def turn(speed, angle):
    reset()
    while True:
        current = (left_motor.angle() - right_motor.angle()) / 2
        error = angle - current
        if abs(error) < 2:
            break
        correction = PD(angle, current)
        run(-speed + correction, speed - correction)
    stop()

def follow(speed, distance):
    reset()
    target = 50
    while (abs(left_motor.angle()) + abs(right_motor.angle())) / 2 < distance:
        light = floor.reflection()
        correction = PD(target, light)
        run(speed - correction, speed + correction)
    stop()

# ================= TUNING CONSTANTS =================
SPOT  = 120   # CAL: motor-degrees between two neighbouring notes
ZONE  = 90    # CAL: short placing move inside the colour zone
LOAD  = 95    # speed while finding/loading notes (light, can go fast)
CARRY = 70    # speed while CARRYING a batch (gentler so nothing topples)
TURN  = 70    # turning speed (calmer = more accurate)
# Carry distance read off the mat photo: staff centroid (1464,540) -> bottom-left
# zones (~250,950) ≈ 1280 mm, and one note-spacing ≈ 171 mm, so ≈ 7.5 spacings.
# Expressed in SPOT units so it scales with YOUR wheel just like every other move.
CARRY_DIST = round(7.5 * SPOT)   # ≈ 900 motor-degrees (CAL: ~1280 mm on the mat)

# ================= MISSION 1: PLAY THE SONG (120 pts) =================
# REALITY: the arms hold 3 per trip (stack 2 in front + push 1 with back), and
# the matching colour zones are at the BOTTOM-LEFT — so each batch must be
# CARRIED there. Six notes = exactly TWO collect→carry→drop trips (same shape
# as the coach's script; the speed comes from HOW each trip is driven):
#   * drive STRAIGHT to each note (no spot-by-spot counting crawl)
#   * run the empty return leg fast, the loaded carry leg gently
def grab_note(distance, arm):
    move(LOAD, distance)   # straight to the note — no crawling
    arm.dc(100)            # capture it (front = stack, back = push)
    wait(90)

def deliver_batch(carry_dist):
    # RULE NOTE (organizer update, 5 Jun 2026): a note only scores while UPRIGHT,
    # and the rules warn that holding it at the end "can lift it slightly" and fail
    # the upright check. So fully RELEASE each note and BACK OFF — never end a run
    # still gripping one. "Completely in": seat each note inside ONE zone only.
    turn(TURN, -90)              # CAL: face the bottom-left zones
    move(CARRY, carry_dist)      # CAL: carry the 3 notes down-left (gentle)
    back.dc(-100)                # drop the pushed note, upright, in its colour zone
    move(CARRY, ZONE); front.dc(-100)   # release a stacked note, upright
    move(CARRY, ZONE); front.dc(-100)   # release the other, upright
    move(-CARRY, 60)             # back off so the arm clears the notes (stay upright)

def play_the_song():
    # ---- Trip 1: the three nearest notes ----
    turn(TURN, -90)              # CAL: onto the staff
    grab_note(120, front)       # CAL: note 1 -> stack (front)
    grab_note(SPOT, front)      # CAL: note 2 -> stack (front)
    grab_note(SPOT, back)       # CAL: note 3 -> push  (back)
    deliver_batch(CARRY_DIST)          # CAL: carry + drop at the bottom-left zones
    # ---- Trip 2: back for the far three notes ----
    move(-LOAD, CARRY_DIST)            # CAL: empty return to the staff (fast)
    turn(TURN, 90)
    grab_note(SPOT, front)      # CAL: note 4 -> stack
    grab_note(SPOT, front)      # CAL: note 5 -> stack
    grab_note(SPOT, back)       # CAL: note 6 -> push
    deliver_batch(CARRY_DIST)          # CAL: carry + drop

# ================= MISSION 2: INSTRUMENTS (45 pts) =================
# Curve down to the truck, capture the three instruments on the back arm and
# push them straight into the backstage box. No "upright" needed — just push.
def prepare_instruments():
    turn(TURN, 45)         # CAL: aim at the truck (bottom-centre)
    move(LOAD, 430)        # CAL: in behind the instruments
    back.dc(70)            # extend the pusher behind all three
    turn(TURN, 90)         # CAL: face the backstage (left)
    move(LOAD, 360)        # CAL: shove them into the backstage box
    back.dc(-70)

# ================= MISSION 3: MICROPHONE (20 pts) =================
# The mic also starts in the truck; set it upright in its target circle.
def place_microphone():
    move(-LOAD, 120)       # CAL: ease back off the backstage
    turn(TURN, -90)        # CAL: turn to the mic target
    move(60, 150)          # CAL: gently nose the mic into its circle
    front.dc(100)          # tip it upright
    wait(150)
    front.dc(-100)

# ================= MISSION 4: CABLES (30 pts) =================
# Cheapest points, so last — the stage is at the far left. Use follow() to ride
# the stage-front line accurately, then lay each cable into its grey area.
def connect_cables():
    turn(TURN, 180)        # CAL: face along the stage front
    follow(LOAD, 300)      # CAL: ride the line to the upper cable area
    front.dc(100); wait(80)   # seat cable 1
    front.dc(-100)
    move(-LOAD, 560)       # CAL: down to the lower cable area
    front.dc(100); wait(80)   # seat cable 2
    front.dc(-100)

# ================= MAIN =================
def main():
    timer.reset()

    play_the_song()        # 120 — bank the big points first
    prepare_instruments()  #  45
    place_microphone()     #  20
    connect_cables()       #  30
    # Bonus (40) is free: we never touch the clef / amp / speakers.

    stop()
    hub.display.number(int((120000 - timer.time()) / 1000))   # spare seconds


main()