need comments and review on customized PID

[familynygood]

pid.py
Hello. Dear Pybricks contributors,
Can you please comment the strength and weakness of the attached customized PID codes compared with pybricks's PID? A student's parent claimed that pybricks's wheel balancing use only P control and has limitations. Is it true?

[dlech]

Here is a copy of the file to make it easier for people to see it.

from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import StopWatch
from pybricks.tools import wait, multitask, run_task

oh = PrimeHub()
motor = Motor(Port.F, Direction.COUNTERCLOCKWISE) #left attachment
motor_2 = Motor(Port.E, Direction.CLOCKWISE) #left drive
motor_3 = Motor(Port.A, Direction.COUNTERCLOCKWISE) #right drive
motor_4 = Motor(Port.D, Direction.CLOCKWISE) #right attachment
diameter=62
width=140
drive_base = DriveBase(motor_2, motor_3, diameter, width)

integral=0
derivative=0
lastError=0
wheel=1/360*62*3.1415926 # 1 degree corresponds to a distance of (wheel) mm
Kps=1
Kpt=1

def pid_correction(target, heading, Kp, Ki, Kd):
    #pamerameters: target heading, measured heading, proportional, integral, derivative
    global lastError #previous error
    global integral #sum of errors
    global derivative #difference of two errors
    error = heading-target
    integral = integral+error
    derivative = error-lastError
    correction = Kp * error + Ki * integral + Kd * derivative #sum of three pid terms
    lastError = error
    return correction

def gyro_pid(direction, distancemm, angle, speed, sensor, Kp, Ki, Kd, slowdown, accel):
    global lastError
    global integral
    global derivative
    lastError = 0
    integral = 0
    derivative = 0
    speed_f=speed #real speed
    slowdowndistance = 0.2 * speed #distance to trigger slowing down
    motor_2.reset_angle(0)
    motor_3.reset_angle(0)
    tolerence=0.5 #position precision tolerence
    wait_flag=0 #jump out of distance control if stuck
    if direction == "backward":
        while (motor_2.angle()+motor_3.angle())*wheel/2<distancemm- tolerence and speed_f>1.5:
            dis_travel = (motor_2.angle()+motor_3.angle())*wheel/2 #travel distance calculation
            speed_f=speed #real speed
            if distancemm-dis_travel < slowdowndistance and slowdown==1:
                speed_f = speed*(distancemm-dis_travel)/slowdowndistance*Kps+25 #control the heading speed if within the slow down distance
            #print('heading_gyro',oh.imu.heading())
            #print('motor_2_angle', motor_2.angle())
            #print('motor_3_anlge', motor_3.angle())
            #print('heading_encoder',(motor_3.angle()-motor_2.angle())*diameter/width/2)
            if sensor=="gyro":
                heading=oh.imu.heading() #heading measured by gyro
            elif sensor=="encoder":
                heading=(motor_3.angle()-motor_2.angle())*diameter/width/2 #heading measured by encoder
            correction = pid_correction(angle, heading, Kp, Ki, Kd) #call pid for wheel balancing
            #print('correction',correction)
            motor_2.run(speed_f+correction/speed*speed_f) #adjust left wheel speed
            motor_3.run(speed_f-correction/speed*speed_f) #adjust right wheel speed
            #print('distance',(motor_2.angle()+motor_3.angle())*wheel/2)
        motor_2.hold()
        motor_3.hold()
    if direction == "forward":
        while (motor_2.angle()+motor_3.angle())*wheel/2 > -distancemm+tolerence and speed_f > 1.5 and wait_flag==0:
            dis_travel = (motor_2.angle()+motor_3.angle())*wheel/2 #travel distance calculation
            speed_f=speed #real speed
            if -distancemm-dis_travel > -slowdowndistance and slowdown == 1:
                speed_f=(speed)*(distancemm+dis_travel)/ slowdowndistance/Kps+25 #control the heading speed if within the slow down distance
            print('heading_gyro',oh.imu.heading())
            print('motor_2_angle', motor_2.angle())
            print('motor_3_anlge', motor_3.angle())
            print('heading_encoder',(motor_3.angle()-motor_2.angle())*diameter/width/2)
            if sensor=="gyro":
                heading=oh.imu.heading() #heading measured by gyro
            elif sensor=="encoder":
                heading=(motor_3.angle()-motor_2.angle())*diameter/width/2 #heading measured by encoder
            correction = pid_correction(angle, heading, Kp, Ki, Kd) #call pid for wheel balancing
            if abs(motor_2.speed(window=200))<0.0001 and abs(motor_2.angle()) >10:
                wait_flag=1
            print('correction',correction)
            motor_2.run(-speed_f+correction/speed*speed_f) #adjust left wheel speed
            motor_3.run(-speed_f-correction/speed*speed_f) #adjust right wheel speed
            print('distance',(motor_2.angle()+motor_3.angle())*wheel/2)
        motor_2.hold()
        motor_3.hold()