First "working" fish collector

#!/usr/bin/env pybricks-micropython
from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
                                 InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.nxtdevices import (LightSensor)
from pybricks.nxtdevices import ColorSensor as NXTColorSensor
#from pybricks import nxtdevices.ColorSensor
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 = NXTColorSensor(Port.S2) # Faces fish
color_sensor_two = ColorSensor(Port.S3) # Faces down
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 = 30

driver = DriveBase(left_motor, right_motor, wheel_diameter, axle_track)
driver.settings(400, 200, 30, 1200)
fish_collected = 0
amount_per_pond = 4
line_counter = 0
previous_state = False
# Write your program here.
ev3.speaker.beep()
wait(300)

def print_on_screen(text):
    ev3.screen.clear()
    ev3.screen.print(str(text))

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

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

    # Calculates each color's proportion of the total light returned
    total_color = read_red_percent + read_green_percent + read_blue_percent
    if total_color == 0:
        red_of_total = 0
        green_of_total = 0
        blue_of_total = 0
    else:
        red_of_total = read_red_percent/total_color
        green_of_total = read_green_percent/total_color
        blue_of_total = read_blue_percent/total_color

    # Returns the requested value
    if return_proportional:
        if color_to_read == "red":
            return 100*red_of_total
        if color_to_read == "green":
            return 100*green_of_total
        if color_to_read == "blue":
            return 100*blue_of_total
    else:
        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(tape_limit):
    # Fucky thing to only set previous_state to False the first time this runs
    try:
        previous_state
    except NameError:
        previous_state = False
        line_counter = 0

    # Prints the line counter to the screen
    print_on_screen(line_counter)

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

    # If the measurement is lower than 35, it must be over a line of tape
    if line_reading < 35:
        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
    if line_counter < tape_limit:
        return True
    else:
        line_counter = 0
        return False

# function for collecting Color.GREEN fish
def 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(95)
    driver.straight(25)

    # 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, -145, then=Stop.HOLD, wait=True)
    wait(1000)

    # Close claw
    claw.track_target(-95)
    wait(200)
    claw.hold()
    wait(1000)

    # Lift tray
    lift_motor.run_angle(100, 105, then=Stop.HOLD, wait=True)
    claw.track_target(0)
    wait(200)
    lift_motor.run_angle(300, 40, then=Stop.HOLD, wait=True)

    # Drive back and rotate to original heading
    driver.straight(-25)
    driver.turn(-95)

def circle_pond(radius, maxlines):
    # Loops while the line counter is under a set amount

    while line_counter_check(maxlines):
        # 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)

        blue = down_color_percent("blue", False)
        green = down_color_percent("green", False)

        # Checks for green fish
        if color_sensor.color() == Color.GREEN:
            green_fish()
            print_on_screen("saw fish!")

        #line_count= line_counter_check(maxlines)
        #if line_counter_check(maxlines) == False
           # driver.stop()

        elif (blue - green >= 1):


            turn_rate = (((8 / radius)*(blue - green))*400) -40

            #if turn_rate > 90:
             #   turn_rate = 90



            # Calculate the deviation from the midpoint.
            #deviation = color_deviation - 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)
            #driver.curve(320, 360)

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

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


def drive_to_next_pond():
    #print_on_screen("to next pond...")

    # Drives forward until blue portion is higher than 55%
    down_blue = down_color_percent("blue", True)
    while down_blue < 55:
        driver.drive(DRIVE_SPEED, 0)
        print_on_screen(down_blue)

        down_blue = down_color_percent("blue", True)
        wait(10)

    # When a pond is reached, stop all motion
    driver.drive(DRIVE_SPEED, 0)
    wait(300)
    driver.stop()
    #driver.drive(DRIVE_SPEED, 0)
    #driver.straight(100, then=Stop.HOLD, wait=True)


def return_dock():
    print_on_screen("to dock...")
    driver.turn(-90)
    driver.straight(30)
    driver.turn(-90)
    driver.straight(-300)
    driver.stop()

#drive_to_next_pond()
circle_pond(254, 7)
drive_to_next_pond()
circle_pond(152.4, 4)
drive_to_next_pond()
circle_pond(203.2, 5)
return_dock()