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 gyro-turn.py 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()
imu.reset()
imu.enable_default()
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():
time.sleep_ms(300)
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():
imu.gyro.read()
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
motors.off()
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():
imu.gyro.read()
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)
'''
motors.off()
def turn(angle):
global drive_motors, robot_angle
global last_time_far_from_target, last_time_gyro_reading, robot_angle
motors.off()
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():
imu.gyro.read()
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)
else:
motors.off()
time.sleep_ms(150)