Micropython EV3 Functions

# General Purpose Functions
#!usr/bin/env pybricks-micropython
from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, ColorSensor, GyroSensor)
from pybricks.robotics import DriveBase
from pybricks.parameters import Port, Color, Stop

# Objects
robot = DriveBase(Motor(Port.A), Motor(Port.B), wheel_diameter = 142.24, axle_track = 152)
color_sensor = ColorSensor(Port.S1)
color_sensor2 = ColorSensor(Port.S2)
gyro_sensor = GyroSensor(Port.S3)
left_wheel = Motor(Port.A)
right_wheel = Motor(Port.B)
ev3 = EV3Brick()


# PROGRAM
## Color Sensor
### PID Line Follower
def follow_line_pid(threshold):
    integral = 0
    last_error = 0

    kp = 2
    ki = .09
    kd = .5

    while True:
        error = threshold - color_sensor.reflection()
        integral += error
        derivative = error - last_error
        
        proportional_gain = error * kp
        integral_gain = integral * ki
        derivative_gain = derivative * kd
        
        correction = proportional_gain + integral_gain + derivative_gain
        robot.drive(50, correction)
        
        error = last_error

### PID Double Line Follower
def double_follow_line_pid():
    integral = 0
    last_error = 0
    
    kp = 2
    ki = 0.09
    kd = 0.5
    
    while True:
        error = color_sensor.reflection() - color_sensor2.reflection()
        integral += error
        derivative = error - last_error
        
        proportional_gain = error * kp
        integral_gain = integral * ki
        derivative_gain = derivative * kd
        
        correction = proportional_gain + integral_gain + derivative_gain
        robot.drive(50, correction)
        
        last_error = error


## Gyro Sensor
### Proportional Gyrostrate
def gyrostraight_proportional(angle, distance):
    gyro_sensor.reset_angle(angle)
    
    while robot.distance() != distance:
        direction = gyro_sensor.angle() * -10
        robot.drive(50, direction)
    
    right_wheel.hold()
    left_wheel.hold()
    robot.drive(0, 0)

### PID Gyrostrate
def gyrostraight_pid(angle, distance):
    gyro_sensor.reset_angle(angle)
    
    integral = 0
    last_error = 0
        
    kp = 2
    ki = 0.09
    kd = 0.5
    
    while robot.distance() != distance:
        error =  angle - gyro_sensor.angle()
        derivative = error - last_error
        integral += error
        
        proportional_gain = error * kp
        integral_gain = integral * ki
        derivative_gain = derivative * kd
        
        correction = proportional_gain + integral_gain + derivative_gain
        robot.drive(100, correction)
    
    robot.drive(0, 0)
    right_wheel.hold()
    left_wheel.hold()
    
### PID Perfect Turn
def perfect_turn(turn_value):
    gyro_sensor.reset_angle(0)
    last_error = 0

    kp = 2
    ki = 0
    kd = 0.49

    if turn_value > 0:
        while not gyro_sensor.angle() > (turn_value - 1):
            error = turn_value - gyro_sensor.angle()
            derivative = error - last_error

            proportional_gain = error * kp
            derivative_gain = derivative * kd

            correction = proportional_gain + derivative_gain
            left_wheel.run(correction + 5)
            right_wheel.run(-correction - 5)

            error = last_error
        
        robot.drive(0, 0)
        right_wheel.hold()
        left_wheel.hold()

    else:
        while not gyro_sensor.angle() < (turn_angle + 1):
            error = turn_value - gyro_sensor.angle()
            derivative = error - last_error

            proportional_gain = error * kp
            derivative_gain = derivative * kd
            correction = proportional_gain + derivative_gain

            left_wheel.run(-correction - 5)
            right_wheel.run(correction + 5)

            error = last_error

        robot.drive(0, 0)
        right_wheel.hold()
        left_wheel.hold()

### 2nd PID Perfect Turn
def turn(turn_value):
    robot.reset()
    gyro_sensor.reset_angle(0)
    right_wheel.reset_angle()
    left_wheel.reset_angle()
    
    integral = 0
    last_error = 0

    kp = 2
    ki = 0.0085
    kd = 0.5

    while abs(gyro_sensor.angle() - turn_value) >= 1:
        error = turn_value - gyro_sensor.angle()
        derivative = error - last_error
        last_error = error
        integral += error

        proportional_gain = error * kp
        integral_gain = integral * ki
        derivative_gain = derivative * kd

        correction = proportional_gain + integral_gain + derivative_gain
        right_wheel.run(-correction - 9)
        left_wheel.run(correction + 9)
    
    robot.drive(0, 0)
    left_wheel.hold()
    right_wheel.hold()