GiggleBot Light Follower w/ Proportional Controller & LPF

gigglebot_light_follower.py

from microbit import *
from gigglebot import *
from utime import sleep_ms, ticks_us

motor_speed = 100
update_rate = 70
setpoint = 0.5
Kp = 65.0

integral = 0.0
tau = 0.005
dt = 1.0 / update_rate
alpha = tau / (tau + dt)

run = True

def lpf(value):
    '''
    Low Pass Filter
    '''
    global integral, alpha
    integral = alpha * value + (1 - alpha) * integral
    return integral

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 light follower
    if button_b.is_pressed():
        run = False
        stop()
        sleep_ms(500)

    if run is True:
        start_time = ticks_us()

        # read the light sensors
        right, left = read_sensor(LIGHT_SENSOR, BOTH)

        # light is on the left when position < 0.5
        # light is on the right when position > 0.5
        # light 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

        # apply low pass filter, then use a P controller
        error = lpf(position) - setpoint
        correction = -error * Kp

        # calculate motor speeds
        leftMotorSpeed = motor_speed + correction
        rightMotorSpeed = motor_speed - correction
        
        # clipping 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((position, lpf(position)))

        # 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)