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