Gryo Error - Precision Turning

I’m coding a 3pi+ 2040 for a robotics competition heavily focused on precision. I have my code below for the straight (moveg) and turn functions. Over the course of 20 or so turns I find myself losing quite a few degrees despite calibrating the gyro at the beginning and using the gyro during turns. The code is heavily borrowed from the built-in function. I’ve tried setting a max speed to avoid slipping of the wheels and have even tried reducing the margin of error from 3 to 2 but now it’s causing random pauses, especially at lower speed. What can I do to optimize my turns? Any tips for optimizing my straight PID function would also be appreciated because I know my code isn’t optimal, I just couldn’t find many references for a closed-loop PID function for this robot.

from pololu_3pi_2040_robot import robot
from pololu_3pi_2040_robot.extras import editions
import time

angle_to_turn = 90

motors = robot.Motors()
encoders = robot.Encoders()
button_a = robot.ButtonA()
button_c = robot.ButtonC()
display = robot.Display()

imu = robot.IMU()

max_speed = 3000
kp = 140
kd = 4

drive_motors = False
last_time_gyro_reading = None
last_time_acc_reading = None
turn_rate = 0.0  # degrees per second
robot_angle = 0.0  # degrees
target_angle = 0.0
last_time_far_from_target = None

def calibrate(): 
    calibration_start = time.ticks_ms()
    stationary_gz = 0.0
    reading_count = 0

    while time.ticks_diff(time.ticks_ms(), calibration_start) < 1000:
        if imu.gyro.data_ready():
            stationary_gz += imu.gyro.last_reading_dps[2]
            reading_count += 1
    stationary_gz /= reading_count

def moveg(dist):
    global drive_motors
    global last_time_far_from_target, last_time_gyro_reading, last_time_acc_reading, robot_angle
    target_angle = robot_angle
    sl = 511*speed
    sr = 500*speed
    travel = 0
    encoders.get_counts(reset = True)
    while travel < dist:
        if imu.gyro.data_ready():
            turn_rate = imu.gyro.last_reading_dps[2]  # degrees per second
            now = time.ticks_us()
            if last_time_gyro_reading:
                dt = time.ticks_diff(now, last_time_gyro_reading)
                robot_angle += turn_rate * dt / 1000000
            last_time_gyro_reading = now
        c = encoders.get_counts()
        travel = c[0]*10.5
        travel /= 360
        motors.set_speeds(sl + 15*(robot_angle - target_angle), sr + 15*(target_angle - robot_angle))

    if abs(target_angle - robot_angle) > 1:
        turn(target_angle - robot_angle)

def turn(angle):
    global drive_motors, robot_angle
    global last_time_far_from_target, last_time_gyro_reading, robot_angle
    drive_motors = not drive_motors
    max_speed1 = 1500
    #og kp = 140 kd = 4
    kp = 140
    kd = 4
    turn_rate = 0
    target_angle = robot_angle + angle
    while drive_motors:
        if imu.gyro.data_ready():
            turn_rate = imu.gyro.last_reading_dps[2]  # degrees per second
            now = time.ticks_us()
            if last_time_gyro_reading:
                dt = time.ticks_diff(now, last_time_gyro_reading)
                # initial value over dt 1000000
                robot_angle += turn_rate * dt / 1000000
            last_time_gyro_reading = now

        if drive_motors:
            far_from_target = abs(robot_angle - target_angle) > 2
            if far_from_target:
                last_time_far_from_target = time.ticks_ms()
            elif time.ticks_diff(time.ticks_ms(), last_time_far_from_target) > 250:
                drive_motors = False

        if drive_motors:
            turn_speed = (target_angle - robot_angle) * kp - turn_rate * kd
            if turn_speed > max_speed1: turn_speed = max_speed1
            if turn_speed < -max_speed1: turn_speed = -max_speed1

            motors.set_speeds(-turn_speed, turn_speed)

I think I found the source of my issue. When my robot is static and I’m running the function, the robot angle it is reporting is steadily increasing by almost a couple of degrees per second despite the robot not moving. Is there a way to address this? Both of the Polulu 3pi+ I own have this passive increase. Other feedback on the code is still appreciated.


What described in your second post sounds like gyroscope drift, which is an inherent property of all gyros. Just to make sure that is what’s happening (and that there is not some more unusual problem going on), could you post a simplified, but still complete version of your program that demonstrates the problem along with some notes describing (or perhaps a video showing) the amount of error that typically accumulates when you run the program? The forum does not allow users to post very large videos, but it does work well with videos linked from other sites (like YouTube or Vimeo).

- Patrick