Drive between ponds

from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
                                 InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import Port, Stop, Direction, Button, Color
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile


# This program requires LEGO EV3 MicroPython v2.0 or higher.
# Click "Open user guide" on the EV3 extension tab for more information.


# Create your objects here.
ev3 = EV3Brick()
right_motor = Motor(Port.B)
left_motor = Motor(Port.C)
lift_motor = Motor(Port.A)
claw = Motor(Port.D)
light_sensor = LightSensor(Port.S1)
color_sensor = ColorSensor(Port.S2)
color_sensor_two = ColorSensor(Port.S3)
wheel_diameter = 43.3
axle_track = 130

PROPORTIONAL_GAIN = 1.2
DRIVE_SPEED = 75
#RAD_TO_DEG = 57.29578
HIGH_READING = 40
LOW_READING = 30
midpoint = (HIGH_READING + LOW_READING)/2

driver = DriveBase(left_motor, right_motor, wheel_diameter, axle_track)
driver.settings(800, 200, 30, 1200)
fish_collected = 0
amount_per_pond = 4
line_counter = 0

# Write your program here.
ev3.speaker.beep()
wait(300)

def down_color_percent(color_to_read):
    # Reads the RBG value from the downwards-facing color sensor
    reading = color_sensor_two.rgb()

    # Separates the tuple into three variables
    read_red_percent = str(color_reading[0])
    read_green_percent = str(color_reading[1])
    read_blue_percent = str(color_reading[2])

    # Returns the requested value
    if color_to_read == "red":
        return read_red_percent
    if color_to_read == "green":
        return read_green_percent
    if color_to_read == "blue":
        return read_blue_percent

def line_counter_check():
    # Reads value from light sensor, returns integer on scale from 1-100
    line_reading = line_sensor.reflection()

    # If the measurement is lower than ___, it must be over a line of tape
    if line_reading < !!!MEASURE:
        over_line = True
    else:
        over_line = False

    # If the current state is  different from the last state, so it's only run when something actually changes
    if previous_state != over_line:
        # Only increment the line counter when the over_line variable changes from False to True
        if over_line:
            line_counter = line_counter + 1

        # This is used to keep track of what the last reading's result was
        previous_state = over_line

def green_fish(): # function for collecting Color.GREEN fish
# Stops any motion that's in progress
    driver.stop()

# Drive back so axis of rotation lines up with fish, then rotate 90 degrees and drive forward a bit
    driver.straight(-40)
    driver.turn(90)
    driver.straight(20)

# Reset angles
    claw.reset_angle(0)
    lift_motor.reset_angle(0)

# Open claw
    claw.track_target(95)
    wait(200)
    claw.hold()
    wait(1000)

# Lower tray
    lift_motor.run_angle(100, -180, then=Stop.HOLD, wait=True)
    wait(1000)

# Close claw
    claw.track_target(0)
    wait(200)
    claw.hold()
    wait(1000)

# Lift tray
    lift_motor.run_angle(100, 180, then=Stop.HOLD, wait=True)

# Drive back and rotate to original heading
    driver.straight(-20)
    driver.turn(-90)


def circle_pond(radius):
    # Loops while the line counter is under a set amount
    while line_counter < !!!TEST
        # calculates a constant turning rate for a given velocity and radius
        #constant_turn_rate = DRIVE_SPEED/(radius + (0.5 * axle_track))
        #const_turn_rate_degrees = RAD_TO_DEG * (constant_turn_rate)

        # Checks for green fish
        if  color_sensor.color() == Color.Green:
            green_fish()

        # Checks if there's a line to count
        line_counter_check()

        # Calculate the deviation from the midpoint.
        deviation = down_color_percent("blue") - midpoint

        # Calculate the turn rate.
        turn_rate = (PROPORTIONAL_GAIN * deviation) #+ const_turn_rate_degrees

        # Set the drive base speed and turn rate.
        driver.drive(DRIVE_SPEED, turn_rate)

        # You can wait for a short time or do other things in this loop.
        wait(10)

    # When the line counter reaches that amount, stop all motion
    driver.stop()


def drive_to_next_pond():
    # Drives forward until stopped
    driver.drive(DRIVE_SPEED, 0)

    while blue_percent < !!!MEASURE:
        # Read RGB values from sensor, returns tuple of (presumably) [red%, green%, blue%]
        blue_percent = down_color_percent("blue")

    # When a pond is reached, stop all motion
    driver.stop()


#drive_to_next_pond()
#circle_pond(203.2) # medium pond

#drive_to_next_pond()
#circle_pond(152.4) # small pond

drive_to_next_pond()
circle_pond(254) # large pond