Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
GiggleBot PID Line Follower Tuner (requires a remote to tune it) - xjfls23
from microbit import *
from gigglebot import *
from utime import sleep_ms, ticks_us
import radio
import ustruct
# initialize radio and GB neopixels
radio.on()
neo = init()
# timing
update_rate = 70
# default gain values
Kp = 0.0
Ki = 0.0
Kd = 0.0
setpoint = 0.5
trigger_point = 0.0
min_speed_percent = 0.2
base_speed = 100
last_position = setpoint
integral = 0.0
run_neopixels = False
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
total_time = 0.0
total_counts = 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
display.scroll('{} - {}'.format(total_time, total_counts), delay=100, wait=False)
total_time = 0.0
total_counts = 0
pixels_off()
stop()
sleep_ms(500)
if run is True:
# read the line sensors
start_time = ticks_us()
# check if we have updated the Kp/Kd gains with a remote
try:
Kp, Ki, Kd, trigger_point, min_speed_percent = ustruct.unpack('fffff', radio.receive_bytes())
set_eyes()
except TypeError:
pass
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
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 to show to which direction the GiggleBot has to go
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] = (12, 44, 41)
else:
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(12 * percent), int(44 * percent), int(41 * percent))
else:
# neo[center_pixel - i] = tuple(map(lambda x: int(x * percentage), turquoise))
neo[center_pixel - i] = (int(12 * percent), int(44 * percent), int(41 * percent))
break
neo.show()
try:
# clip the motors
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
total_time += delay_diff
total_counts += 1
if 1000.0 / update_rate - delay_diff > 0:
sleep(1000.0 / update_rate - delay_diff)
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.