GiggleBot PID Line Follower Tuner (requires a remote to tune it) - xjfls23

gigglebot_line_follower_tuner.py

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)