Micropython EV3 Functions

# Gyrostrate and PID Line Following
#!/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)
right_wheel = Motor(Port.B)
left_wheel = Motor(Port.A)
ev3 = EV3Brick()

# PROGRAM

## Color Sensor
### PID Line Follower
def follow_line_pid(threshold):
    integral = 0
    derivative = 0
    last_error = 0
    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
    derivative = 0
    last_error = 0
    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 gyrostrate_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, stop = Stop.HOLD)

### PID Gyrostrate
def gyrostrate_pid(angle, distance):
    gyro_sensor.reset_angle(angle)
    
    derivative = 0
    integral = 0
    last_error = 0
    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
        
        integral_gain = (integral * ki)
        derivative_gain = (derivative * kd)
        proportional_gain = (angle - gyro_sensor.angle()) * kp
        
        correction = (proportional_gain + integral_gain + derivative_gain)
        robot.drive(100, correction)
    
    robot.drive(0, 0, stop = stop.HOLD)
    right_wheel.hold()
    left_wheel.hold()