from microbit import *
import machine
import utime

#motors
pinMotorLeft = pin12
pinMotorRight = pin13
pinMotorLeft.set_analog_period(10)
pinMotorRight.set_analog_period(10)

#distance sensor
pinTrigger = pin14
pinEcho = pin15

def clip(v,vmin,vmax):
    if v > vmax: v = vmax
    if v < vmin: v = vmin
    return v

def setLeftSpeed(v):# v from -100 to 100
    v = clip(v,-100,100)
    pinMotorLeft.write_analog(150+v)

def setRightSpeed(v):# v from -100 to 100
    v = clip(v,-100,100)
    pinMotorRight.write_analog(150-v)

pinEcho.set_pull(pinEcho.NO_PULL) # set no pull to echo

def get_dist():
    # send a pulse
    pinTrigger.write_digital(0)
    utime.sleep_us(2)
    pinTrigger.write_digital(1)
    utime.sleep_us(10)
    pinTrigger.write_digital(0)

    # check echo and time it to determine distance
    d = machine.time_pulse_us(pinEcho,1,11600)
    if d>0:
        return d/58
    else:
        return -1

standBy = False

while True:
    if button_a.was_pressed():
        standBy = not(standBy)
        if standBy:
            setLeftSpeed(0)
            setRightSpeed(0)

    if not(standBy):
        # test distance
        d = int(get_dist()) # take integer part of measure
        displayPositiveNumber(d)# display

        if not(d < 0 or d > 25):
            v = int((d-12)*5)
            setLeftSpeed(v)
            setRightSpeed(v)

    sleep(100)