PID Programming Python

Hello, I have been trying to add PID into my auton, but I can’t figure out how to add it with python programming. as far as I can see, most people use c++.

Find some example code and start writing it in python 1 line at a time. Follow back up here if you get stuck.

2 Likes

this is what i came up with:

o = False
Kp = 0 
Ki = 0 
Kd = 0

error = 0
lastError = 0 
integral = 0
derivative = 0
leftEncoderClicks = leftmotor.position(TURNS)
rightEncoderClicks = rightmotor.position(TURNS)

def drivePID(clicks):
    leftEncoderClicks = leftmotor.position(TURNS)
    rightEncoderClicks = rightmotor.position(TURNS)
    while leftEncoderClicks < clicks: 
        leftEncoderClicks = leftmotor.position(TURNS)
        rightEncoderClicks = rightmotor.position(TURNS)
        error = leftEncoderClicks - rightEncoderClicks
        integral = integral + error
        derivative = error - lastError

        rightPower = leftPower + (error * Kp) 
                          + (integral * Ki) 
                          + (derivative * Kd)
        
        lastError = error;
        left_drive_smart.set_velocity(leftPower,RPM)
        right_drive_smart.set_velocity(leftPower,RPM)
        wait(.2,SECONDS)
    rightpower = 0
    leftpower = 0 
    left_drive_smart.set_velocity(leftPower,RPM)
    right_drive_smart.set_velocity(leftPower,RPM)

im going to test it in school today.

I am having problems with this code, idk what’s wrong but it isn’t driving when I use this function.

So the first immediate problem is you never set LeftPower to anything. But more than that, I am not sure what your goal is here, are you trying to turn in place? Your code is pretty close if your goal is to turn in place, to that you just need to

make

rightPower = leftPower + (error * Kp) 
                          + (integral * Ki) 
                          + (derivative * Kd)
        
        lastError = error;
        left_drive_smart.set_velocity(leftPower,RPM)
        right_drive_smart.set_velocity(leftPower,RPM)

into

        pidOutput = (error * Kp) 
                          + (integral * Ki) 
                          + (derivative * Kd)
        
        lastError = error;
        left_drive_smart.set_velocity(pidOutput,RPM)
        right_drive_smart.set_velocity(-pidOutput,RPM)
1 Like