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();
Please edit your post to add code tags, using the </> button on post editor.
The simplest way to drive straight is to use PID control based on the difference between right and left encoder counts, during one time interval. The PID setpoint is zero.
Something like this usually works well, although you may need to add a Kd term.
error = left_count - right_count;
set_speed_right(base_speed + Kp*error); //if left count is greater that right count, increase right speed
set_speed_left( base_speed - Kp*error);
I initially tried that, but it did not work along with the rest of the code. Here was that 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
#
# dist = 100
# diff = left - right
# kp = 200
#
# if (dist / 12 * 180 > eCount):
# motors.set_speeds(-1 * (5500 - diff * kp), 5500 + diff * kp)
# else:
# motors.off()
#
intent = 500
kp = 1400
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]))*3
motors.set_speeds(-MIN_SPEED_FWD + (kp * diff), MIN_SPEED_FWD + (kp * diff))
motors.off();
display.fill_rect(40, 48, 64, 16, 0)
left_encoder, right_encoder = encoders.get_counts()
display.text(f"{c[0]:8}", 40, 48)
display.text(f"{c[1]:>8}", 40, 56)
display.show()
# currT = time.ticks_ms() / 1000
# deltaT = float(currT - prevT)
#
# velocity = float((eCount - prevE)) / (deltaT + 0.1)
# prevE = eCount
# prevT = currT
#
# target = 200
# targetVelocity = 400
#
# kp = 10
# e1 = targetVelocity - velocity
# u1 = e1 * kp
#
# motors.set_speeds(-u1, u1 + 100)
#
# motors.set_speeds(-4500, 5500)
What does that mean?
diff = ((-1 * c[0]) - (-1 * c[1]))*3
You have an unusual style of programming. What is the point of multiplying values by -1 and by 3?
Perhaps the error term has the wrong sign, in which case the PID corrections will have the wrong sign, leading to wild oscillations.
I’m sorry I’m very new to microPython and just trying to get the code to work. By the “rest of the code”, I meant the lines working to calculate distance. On the Zumo, the motors’ terminals are backwards such that positive and negative are reversed, hence the multiplication by -1. As for the multiplication by 3, that was to offset the difference in speed for the left and right motors since the right motors are underperforming.
The “3” is just one factor of the total value of Kp, which at 3*1400 seem ridiculously high to me. Should a difference of 1 count between right and left encoder values change the motor speeds by 4200?
In any case Kp needs to be tuned for correct performance. Do a web search with the phrase “PID tuning” to find tutorials on how to choose Kp, (and Kd, Ki if you need them). So far, my robots have worked well with just a correctly chosen value for Kp.
This line does not make sense to me. See the 3 lines of code in my post above for sign changes.
motors.set_speeds(-MIN_SPEED_FWD + (kp * diff), MIN_SPEED_FWD + (kp * diff))
Put print statements in the code to see whether variables have the expected values and signs.
Finally, I suggest to change the wiring, and avoid all the sign confusion.
