Turning and Driving PID not working

Programming Support VEXcode V5 Tech Support
, ,
1

Hi so I recently tried to make a PID and I did some research and tried to make one and I think I actually have a PID but something is wrong with it. The pid never stops even when the sensors or encoders will go past the target value and the drive pid is super fast while the turning one is so slow it wont move.

anyways here is the code

Drive_Gear_Ratio = (36/48)

max_speed = 100
min_speed = 20

error = 0
integral = 0
derivative = 0
previous_error = 0

kp = .2
ki = .2
kd = .2
kf = 5

def drive_distance(target_distance):
    global error, integral, derivative, previous_error
    
    # Reset PID variables
    error = 0
    integral = 0
    derivative = 0
    previous_error = 0
    
    # Main PID loop
    while True:
        current_distance = ((3.14159265 * Wheel_Diameter) / 360) * (LeftDrive1111_motor_a.position(DEGREES)) * (Drive_Gear_Ratio)
        error = target_distance - current_distance
        integral = integral + error
        derivative = error - previous_error
        output = kp * error + ki * integral + kd * derivative
        feedforward = kf * target_distance
        speed = max(min_speed, min(max_speed, feedforward + max_speed - abs(output)))
        left_speed = speed
        right_speed = speed
        if error < 0:
            left_speed *= -1
            right_speed *= -1
        LeftDrive1111.set_velocity(left_speed, PERCENT)
        LeftDrive1155.set_velocity(left_speed, PERCENT)
        RightDrive1111.set_velocity(right_speed, PERCENT)
        RightDrive1155.set_velocity(right_speed, PERCENT)
        previous_error = error
        wait(20, MSEC)
        # Break the loop when close enough to the target
        if abs(error) < 1:
            break
            LeftDrive1111_motor_a.set_velocity(0, PERCENT)
            LeftDrive1155_motor_a.set_velocity(0, PERCENT)
            RightDrive1111_motor_a.set_velocity(0, PERCENT)
            RightDrive1155_motor_a.set_velocity(0, PERCENT)

def turn_to_angle(target_angle):
    global error, integral, derivative, previous_error

    # Reset PID variables
    error = 0
    integral = 0
    derivative = 0
    previous_error = 0

    # Main PID loop for turning
    while True:
        current_angle = inertial_10.heading(DEGREES)
        error = target_angle - current_angle
        integral = integral + error
        derivative = error - previous_error
        output = kp * error + ki * integral + kd * derivative
        feedforward = kf * target_angle
        speed = max(min_speed, min(max_speed, feedforward + max_speed - abs(output)))
        left_speed = -speed
        right_speed = speed
        LeftDrive1111_motor_a.set_velocity(left_speed, PERCENT)
        LeftDrive1155_motor_a.set_velocity(left_speed, PERCENT)
        RightDrive1111_motor_a.set_velocity(right_speed, PERCENT)
        RightDrive1155_motor_a.set_velocity(right_speed, PERCENT)

        # Update previous error
        previous_error = error

        # Sleep for a short time to avoid excessive loop execution
        wait(20, MSEC)

        # Break the loop when close enough to the target angle
        if abs(error) < 1:
            break
            LeftDrive1111_motor_a.set_velocity(0, PERCENT)
            LeftDrive1155_motor_a.set_velocity(0, PERCENT)
            RightDrive1111_motor_a.set_velocity(0, PERCENT)
            RightDrive1155_motor_a.set_velocity(0, PERCENT)```

I honestly think I just screwed up how I calculated some things but i'm too stupid to figure out how to fix it. Any help is appreciated I have a tourney this weekend and I really need some win-points.
2

Instead of using While True for your loops, maybe do While not error < 1. If your drive PID is too fast, try re-tuning it. If that doesn’t work, use different variables for both the PID’s (make turn PID variables and drive PID variables)

3

Your PID code seems fine, but you should used different kP kI and kD variables for turning and driving because they are different movements. It sounds like your drive kP is too large and your turn kP is too small. Also, you should put the break in the exit condition after you stop all the motors, because if the break is before then the code stopping the motors will never be run.

To tune your PID code, you should start by setting kP to a random number and kI and kD to zero, then keep increasing kP until the robot slightly overshoots the target. Then increase kD until it stops overshooting. You can then go back to increasing kP again. Repeat until increasing kD no longer stops it overshooting, then set kP and kD back to the last value that worked. You probably won’t need to use kI, but if the program keeps getting stuck near the end or isn’t ending up at the right spot then you can try adding kI.

4

Thank you so his helped a lot I can now tune it. Now that I at least tried to tune it though the turning pod is still really slow and the driving pod instead of overshooting 5 feet over shoots by a foot

5

Also I’d like to add that I literally have zero oscillation and I though I had to tune that out but it wasn’t there to begin with