Micropython EV3

# 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

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

# PROGRAM
## PID Line Follower
def follow_line_pid(threshold):
    proportional_gain = 0
    integral_error = 0
    derivative = 0

    last_error = 0
    error = 0

    kp = 2
    ki = .09
    kd = .5

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


## 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.stop()
    left_wheel.stop()
    robot.stop()
    
    robot.stop()


## 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.stop()
    right_wheel.stop()
    left_wheel.stop()