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

@RobertLucian RobertLucian 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