Raspberry Pi Pico - Fast line following robot

code.py

'''
  How it works:
  - https://vt.tiktok.com/ZSLkkvWB7/
'''

import time
import board
import digitalio
import pwmio
import analogio
import simpleio
from adafruit_motor import motor

led = digitalio.DigitalInOut(board.LED)
led.direction = digitalio.Direction.OUTPUT

button = digitalio.DigitalInOut(board.GP0)
button.direction = digitalio.Direction.INPUT
button.pull = digitalio.Pull.UP

NOTE_G4 = 392
NOTE_A4 = 440
NOTE_B4 = 493
NOTE_D5 = 587
buzzer = board.GP6

m1a = pwmio.PWMOut(board.GP2, frequency=20000, duty_cycle=0)
m1b = pwmio.PWMOut(board.GP3, frequency=20000, duty_cycle=0)
m2a = pwmio.PWMOut(board.GP4, frequency=20000, duty_cycle=0)
m2b = pwmio.PWMOut(board.GP5, frequency=20000, duty_cycle=0)

motor_left = motor.DCMotor(m1a, m1b)
motor_right = motor.DCMotor(m2b, m2a)

maker_line = analogio.AnalogIn(board.GP26)

sen_cal = digitalio.DigitalInOut(board.GP22)
sen_cal.direction = digitalio.Direction.OUTPUT
sen_cal.value = True

motor_left.throttle = 0.2
motor_right.throttle = 0.2

def get_voltage(pin):
    return (pin.value * 3.3) / 65536

def calibrate():
    print("Calibrating...")

    sen_cal.value = False
    time.sleep(2.5)
    sen_cal.value = True

    motor_left.throttle = -0.25
    motor_right.throttle = 0.25
    time.sleep(0.3)
    motor_left.throttle = 0
    motor_right.throttle = 0
    time.sleep(0.3)
    motor_left.throttle = 0.25
    motor_right.throttle = -0.25
    time.sleep(0.6)
    motor_left.throttle = 0
    motor_right.throttle = 0
    time.sleep(0.3)
    motor_left.throttle = -0.25
    motor_right.throttle = 0.25
    time.sleep(0.3)
    motor_left.throttle = 0
    motor_right.throttle = 0

    sen_cal.value = False
    time.sleep(0.5)
    sen_cal.value = True

last_proportional = 0
def line_follow_pd(data, setpoint, speed, kp, kd):
    global last_proportional
    speed_left = speed
    speed_right = speed

    proportional = data - setpoint
    derivative = proportional - last_proportional
    last_proportional = proportional

    power_different = (proportional * kp) + (derivative * kd)

    if power_different < -speed:
        power_different = -speed
    if power_different > speed:
        power_different = speed

    if power_different < 0:
        speed_right = speed_right + power_different
    else:
        speed_left = speed_left - power_different

    motor_left.throttle = speed_left
    motor_right.throttle = speed_right

time.sleep(0.1)

motor_left.throttle = 0
motor_right.throttle = 0

simpleio.tone(buzzer, NOTE_G4, duration=0.1)
simpleio.tone(buzzer, NOTE_D5, duration=0.1)

print("RP2040 - fast line following robot")
print("Press button to calibrate")

while button.value:
    continue

simpleio.tone(buzzer, NOTE_G4, duration=0.1)

time.sleep(1)

calibrate()

print("Press button to run")

while button.value:
    continue

simpleio.tone(buzzer, NOTE_G4, duration=0.14)
simpleio.tone(buzzer, NOTE_A4, duration=0.14)
simpleio.tone(buzzer, NOTE_B4, duration=0.14)
simpleio.tone(buzzer, NOTE_D5, duration=0.14)
time.sleep(0.14)
simpleio.tone(buzzer, NOTE_B4, duration=0.14)
simpleio.tone(buzzer, NOTE_D5, duration=0.28)

time.sleep(1)

count = 0
out_track = 0
speed = 0.5

while True:
    if speed < 0.9:
        count = count + 1
    if count > 500:
        count = 0
        speed = speed + 0.1
        
    data = get_voltage(maker_line)
    #time.sleep(0.1)
    #print("Maker Line: {:.2f}V".format(data))
    if data > 0.3 and data < 3.0:
        out_track = 0
        if data > 2.6 or data < 0.6:
            speed = 0.6
        line_follow_pd(data, 1.6, speed, 1, kd=speed*30)
    else:
        out_track = out_track + 1
        if out_track > 4000:
            motor_left.throttle = 0
            motor_right.throttle = 0
            break