GiggleBot PID Line Follower - Tuned w/ NeoPixels

gigglebot_tuned_line_follower.py

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)

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.