Tuesday final version

#!/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 = ColorSensor(Port.S2) # Faces fish
color_sensor_two = NXTColorSensor(Port.S3) # Faces down
wheel_diameter = 43.3
axle_track = 130
#spkr = Sound()

POND_PROPORTIONAL_GAIN = 4#3.4
LINE_PROPORTIONAL_GAIN = 1
DRIVE_SPEED = 110#75
#RAD_TO_DEG = 57.29578
HIGH_READING = 53
LOW_READING = 42
line_midpoint = (HIGH_READING + LOW_READING)/2
pond_midpoint = 43#50

driver = DriveBase(left_motor, right_motor, wheel_diameter, axle_track)
driver.settings(800, 200, 60, 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):
    global line_counter
    global previous_state
    # 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 < 40:
        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(backup_dist, forward_dist, turn_degrees):
    # 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(0-backup_dist)
    driver.turn(turn_degrees)
    driver.straight(forward_dist)

    # 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(-110)
    wait(400)
    claw.hold()
    #wait(1000)

    # Lift tray
    #ev3.speaker.play_file("Lift load.wav")
    lift_motor.run_angle(100, 145, then=Stop.COAST, wait=True)
    #wait(200)
    claw.track_target(0)
    #wait(200)
    #lift_motor.run_angle(300, 40, then=Stop.HOLD, wait=True)
    ev3.speaker.play_file("new_audio_loud.wav")

    # Drive back and rotate to original heading
    driver.straight(0-forward_dist)
    driver.turn(0-turn_degrees)
    driver.straight(backup_dist)

def circle_pond(radius, maxlines, pond_size):
    global line_counter
    # Loops while the line counter is under a set amount
    if pond_size == "small":
        driver.stop()
        driver.turn(-65) # rotate left X degrees once locating the small pond
    if pond_size == "large":
        driver.stop()
        driver.turn(-40)
    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)

        #print_on_screen(driver.angle())

        blue = down_color_percent("blue", True)
        if blue < 30:
            blue = 30
        #green = down_color_percent("green", False)

        # Checks for green fish
        if color_sensor.color() == Color.GREEN:
            if pond_size == "large":
                green_fish(40, 30, 90)
            elif pond_size == "medium":
                if line_counter == 0 or line_counter == 5:
                    green_fish(42.5, 35, 95) #normally 44.5, 35, 95
                else:
                    green_fish(44.5, 35, 90)
            elif pond_size == "small":
                green_fish(48, 30, 85)
            print_on_screen("saw fish!")

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

        else:# (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 = pond_midpoint - blue
            # Calculate the turn rate.
            turn_rate = (POND_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(two_large):
    #print_on_screen("to next pond...")
    # catches the loop until the color sensor is off the initial pond
    #down_red = down_color_percent("red", True)
    #while down_red < 12:
     #   driver.drive(DRIVE_SPEED, 0)
      #  down_red = down_color_percent("red", True)
       # wait(3000)
    driver.drive(DRIVE_SPEED, 0)
    wait(1000)
    if two_large:

        driver.turn(25)
        driver.drive(DRIVE_SPEED, 0)
        wait(1000)
        #driver.drive(DRIVE_SPEED, 0)

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

        down_blue = down_color_percent("blue", True)
        wait(10)
    #while color_sensor_two.color() != Color.BLUE:
        #driver.drive(DRIVE_SPEED, 0)

    # 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 follow_line_straight():
     # Calculate the deviation from the threshold.
    deviation = light_sensor.reflection() - line_midpoint



    # Calculate the turn rate.
    turn_rate = LINE_PROPORTIONAL_GAIN * deviation

    print_on_screen(deviation)

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

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

def return_dock():
    driver.reset()
    print_on_screen("to dock...")
    driver.straight(-2500)#-305
   # driver.turn(25)
    #driver.straight(-275)
    #driver.turn(-30)
    #while driver.distance() >= -200000000:
    #    follow_line_straight()

    driver.stop()


drive_to_next_pond(False)
circle_pond(203.2, 11, "medium")
drive_to_next_pond(False)
circle_pond(152.4, 9, "small")
drive_to_next_pond(True)
circle_pond(254, 10, "large")
return_dock()
#ev3.speaker.play_file("new_audio_loud.wav")