Last active
March 24, 2019 17:59
-
-
Save RobertLucian/be4bdac3cde18b74566992732c1c368f to your computer and use it in GitHub Desktop.
GiggleBot PID Line Follower Tuner (requires a remote to tune it) - xjfls23
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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