Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
GiggleBot PID Line Follower - Tuned w/ NeoPixels
from microbit import *
from gigglebot import *
from utime import sleep_ms, ticks_us
import ustruct
# initialize GB neopixels
neo = init()
# timing
update_rate = 50
# gains/constants (assuming the battery voltage is around 4.0 volts)
Kp = 25.0
Ki = 0.5
Kd = 35.0
trigger_point = 0.3
min_speed_percent = 0.3
base_speed = 70
setpoint = 0.5
last_position = setpoint
integral = 0.0
run_neopixels = True
center_pixel = 5 # where the center pixel of the smile is located on the GB
# turquoise = tuple(map(lambda x: int(x / 5), (64, 224, 208))) # color to use to draw the error with the neopixels
# turquoise = (12, 44, 41) # which is exactly the above turquoise commented above this
error_width_per_pixel = 0.5 / 3 # max error divided by the number of segments between each neopixel
def upper_bound_linear_speed_reducer(abs_error, trigger_point, upper_bound, smallest_motor_power, highest_motor_power):
global base_speed
if abs_error >= trigger_point:
# x0 = 0.0
# y0 = 0.0
# x1 = upper_bound - trigger_point
# y1 = 1.0
# x = abs_error - trigger_point
# y = y0 + (x - x0) * (y1 - y0) / (x1 - x0)
# same as
y = (abs_error - trigger_point) / (upper_bound - trigger_point)
motor_power = base_speed * (smallest_motor_power + (1 - y) * (highest_motor_power - smallest_motor_power))
return motor_power
else:
return base_speed * highest_motor_power
run = False
previous_error = 0
while True:
# if button a is pressed then start following
if button_a.is_pressed():
run = True
# but if button b is pressed stop the line follower
if button_b.is_pressed():
run = False
integral = 0.0
previous_error = 0.0
pixels_off()
stop()
sleep_ms(500)
if run is True:
# read the line sensors
start_time = ticks_us()
right, left = read_sensor(LINE_SENSOR, BOTH)
# line is on the left when position < 0.5
# line is on the right when position > 0.5
# line is in the middle when position = 0.5
# it's a weighted arithmetic mean
try:
position = right / float(left + right)
except ZeroDivisionError:
position = 0.5
# the range has to be (0, 1) and not [0, 1]
if position == 0: position = 0.001
if position == 1: position = 0.999
# use a PD controller
error = position - setpoint
integral += error
correction = Kp * error + Ki * integral + Kd * (error - previous_error)
previous_error = error
# calculate motor speeds
motor_speed = upper_bound_linear_speed_reducer(abs(error), setpoint * trigger_point, setpoint, min_speed_percent, 1.0)
leftMotorSpeed = motor_speed + correction
rightMotorSpeed = motor_speed - correction
# light up the neopixels according to the given error
if run_neopixels is True and total_counts % 3 == 0:
for i in b'\x00\x01\x02\x03\x04\x05\x06\x07\x08':
neo[i] = (0, 0, 0)
for i in b'\x00\x01\x02\x03':
if abs(error) > error_width_per_pixel * i:
if error < 0:
# neo[center_pixel + i] = turquoise
neo[center_pixel + i] = (12, 44, 41)
else:
# neo[center_pixel - i] = turquoise
neo[center_pixel + i] = (12, 44, 41)
else:
percent = 1 - (error_width_per_pixel * i - abs(error)) / error_width_per_pixel
# light up the current pixel
if error < 0:
# neo[center_pixel + i] = tuple(map(lambda x: int(x * percentage), turquoise))
neo[center_pixel + i] = (int(64 * percent / 5), int(224 * percent / 5), int(208 * percent / 5))
else:
# neo[center_pixel - i] = tuple(map(lambda x: int(x * percentage), turquoise))
neo[center_pixel - i] = (int(64 * percent / 5), int(224 * percent / 5), int(208 * percent / 5))
break
neo.show()
try:
# clip the motor speeds
if leftMotorSpeed > 100:
leftMotorSpeed = 100
rightMotorSpeed = rightMotorSpeed - leftMotorSpeed + 100
if rightMotorSpeed > 100:
rightMotorSpeed = 100
leftMotorSpeed = leftMotorSpeed - rightMotorSpeed + 100
if leftMotorSpeed < -100:
leftMotorSpeed = -100
if rightMotorSpeed < -100:
rightMotorSpeed = -100
# actuate the motors
set_speed(leftMotorSpeed, rightMotorSpeed)
drive()
# print((error, motor_speed))
except:
# in case we get into some unfixable issue
pass
# and maintain the loop frequency
end_time = ticks_us()
delay_diff = (end_time - start_time) / 1000
if 1000.0 / update_rate - delay_diff > 0:
sleep(1000.0 / update_rate - delay_diff)
@RobertLucian

This comment has been minimized.

Copy link
Owner Author

commented Mar 16, 2019

If you want it to run faster, edit the above script and set run_neopixels to False and the following gains like this:

  1. update_rate = 70
  2. Kp = 32.0
  3. Ki = 0.5
  4. Kd = 0.8
  5. trigger_point = 0.3
  6. min_speed_percent = 0.2
  7. base_speed = 100

To see to which variables I'm referring, check this link.

Both of the above examples assume the batteries are around ~4.0 volts and that the MicroPython runtime version is 0.4.0.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.