Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
GiggleBot Light Follower w/ Proportional Controller & LPF
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)
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.