ZOOM:BIT line following robot and detect junctions.

main.py

import time
from microbit import *
import music
import rekabit

rekabit.init()

def get_voltage():
    return (pin1.read_analog() * 3.3) / 1023

def motor_speed(left, right):
    rekabit.run_motor(rekabit.M1, rekabit.Forward, int(left))
    rekabit.run_motor(rekabit.M2, rekabit.Forward, int(right))

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

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

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

    if power_different < -1:
        power_different = -1
    if power_different > 1:
        power_different = 1

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

    motor_speed(speed_left*150, speed_right*150)

def servo_move(init_pos, target_pos):
    change = 0
    if init_pos < target_pos:
        change = 1
    else:
        change = -1
    for pos in range(init_pos, target_pos, change):
        rekabit.sets_servo_position(rekabit.S1, pos)
        time.sleep(0.005)

rekabit.sets_servo_position(rekabit.S1, 76)
motor_speed(0, 0)

print("Robot ready!")
display.show(Image.HAPPY)

follow_line = False
left_count = 0
right_count = 0
cross_count = 0

while True:
    if button_a.was_pressed():
        follow_line = not follow_line
        if follow_line:
            print("Move!")
            display.show(Image.ARROW_S)
            music.play(music.POWER_UP)
        else:
            print("Stop!")
            motor_speed(0, 0)

    maker_line = get_voltage()
    ir_left = pin0.read_analog()
    ir_right = pin2.read_analog()
    #print("Maker Line: {:.2f}".format(maker_line))
    #print("IR Left: {:.2f}".format(ir_left))
    #print("IR Right: {:.2f}".format(ir_right))
    #sleep(1000)
    
    if follow_line == True:
        if ir_left > 700 and ir_right < 700: # Left junction
            right_count = 0
            cross_count = 0
            if left_count < 100:
                left_count += 1
            if left_count > 50:
                motor_speed(0, 0)
                display.show(Image.ARROW_E)
                servo_move(76, 170)
                music.play(music.BA_DING)
                time.sleep(1)
                motor_speed(100, 100)
                servo_move(170, 76)
                display.show(Image.ARROW_S)
        elif ir_left < 700 and ir_right > 700: # Right junction
            left_count = 0
            cross_count = 0
            if right_count < 100:
                right_count += 1
            if right_count > 50:
                motor_speed(0, 0)
                display.show(Image.ARROW_W)
                servo_move(76, 0)
                music.play(music.BA_DING)
                time.sleep(1)
                motor_speed(100, 100)
                servo_move(0, 76)
                display.show(Image.ARROW_S)
        elif ir_left > 700 and ir_right > 700: # Cross junction
            left_count = 0
            right_count = 0
            if cross_count < 100:
                cross_count += 1
            if cross_count > 50:
                motor_speed(0, 0)
                display.show(Image('00900:'
                                   '00900:'
                                   '99999:'
                                   '00900:'
                                   '00900'))
                music.play(music.BA_DING)
                time.sleep(1)
                motor_speed(100, 100)
                time.sleep(0.5)
                display.show(Image.ARROW_S)
        elif maker_line > 3:
            motor_speed(100, 100)
        else:
            line_follow_pd(maker_line, 1.6)