Hello! I am trying to develop a PID for a Zumo 2040 to travel in a straight line to any distance from 7-10 meters to an accuracy of 0.1 cm. We have the distance working, but the Zumo is swerving and the right motor is not keeping up with the left despite our best attempts to do so. Does anyone have example code for a PID?
#current code
from zumo_2040_robot import robot
import time
display = robot.Display()
button_a = robot.ButtonA()
button_b = robot.ButtonB()
button_c = robot.ButtonC()
motors = robot.Motors()
encoders = robot.Encoders()
imu = robot.IMU()
imu.reset()
imu.enable_default()
# display.text("A", 8, 28)
# display.text("C", 112, 28)
# display.text("L: ", 24, 48)
# display.text("R: ", 24, 56)
CLICKS_PER_ROTATION = 180
GEAR_RATIO = 15
WHEEL_CIRCUMFERENCE = 2 * 4 * 3.14
MIN_SPEED_FWD = 5000
MAX_SPEED = 2100
eCount = 0;
left_dir = 1
right_dir = 1
left_speed = 0
right_speed = 0
prevT = 0
leftPrev = 0
rightPrev = 0
prevE = 0
deltaT = 0
def d1():
return eCount / (CLICKS_PER_ROTATION * GEAR_RATIO) * WHEEL_CIRCUMFERENCE
while True:
c = encoders.get_counts()
left = -1 * c[0]
right = -1 * c[1]
eCount = (c[0] + c[1]) / 2
currT = time.ticks_us()
deltaT = (currT - prevT) / (1000000.0)
velocity = (eCount - prevE) / deltaT
prevE = eCount
prevT = currT
target = 200
targetVelocity = 1000
kp = 1
e1 = targetVelocity - velocity
u1 = e1 * kp
motors.set_speeds(-u1, u1)
display.fill_rect(40, 48, 64, 16, 0)
left_encoder, right_encoder = encoders.get_counts()
display.text(f"{u1:>8}", 40, 48)
display.text(f"{right_encoder:>8}", 40, 56)
display.show()
# c = encoders.get_counts()
# eCount = -1 * (c[0] + c[1]) / 2
# while (intent / 12 * 180 > eCount):
# c = encoders.get_counts()
# eCount = -1 * (c[0] + c[1]) / 2
# diff = (-1 * c[0]) - (-1 * c[1])
# motors.set_speeds(-MIN_SPEED_FWD + (kp * diff), MIN_SPEED_FWD + (kp * diff))
# motors.off();
6 posts - 2 participants