Hello so I made this PID and it uses a global rotation variable but when its global it only turns right. When the variable is reset at the start of each loop it works just fine but I need it more accurate than that.
def turn_to_angle(target_angle, direction):
global error, integral, derivative, previous_error
# Reset PID variables
error = 0
integral = 0
derivative = 0
previous_error = 0
consecutive_stable_iterations = 0
Time = 3
TimeStop = 50
# Main PID loop for turning
while True:
current_angle = inertial_10.rotation(DEGREES)
error = target_angle - current_angle
if error < 0 :
intergral = 0
derivative = error - previous_error
previous_error = error
output = kp2 * error + ki2 * integral + kd2 * derivative
speed = abs(output)
if error > 1:
left_speed2 = 1 * speed
right_speed2 = -1 * speed
if error < -1:
left_speed2 = -1 * speed
right_speed2 = 1 * speed
if abs(error) < 3:
consecutive_stable_iterations += 1
else:
consecutive_stable_iterations = 0
# Check for stability
if consecutive_stable_iterations >=Time:
LeftDrive1111.stop()
LeftDrive1155.stop()
RightDrive1111.stop()
RightDrive1155.stop()
break
if TimeStop == 1:
wait(3, SECONDS)
break
# Set motor velocities
LeftDrive1111.spin(FORWARD, left_speed2, VOLT)
LeftDrive1155.spin(FORWARD, left_speed2, VOLT)
RightDrive1111.spin(FORWARD, right_speed2, VOLT)
RightDrive1155.spin(FORWARD, right_speed2, VOLT)
# Sleep for a short time to avoid excessive loop execution
wait(10, MSEC)
Also it would be preferable if there was a way to choose the turning direction rather than it turning to the closest point but currently I haven’t gotten either to work.