# Filename to be printed
Thisfile = "WKS31U.py"
#Date  22/2/2022

#############
#  imports  #
#############
from machine import Pin, PWM, ADC
import time, utime
from UKMARS import *   # motors and wall sensors and button

##########
#  data  #
##########
speed = 20             # speed in corridor
mult = 0.002          # amount of steering in corridor

# motors
LeftMotor  = Motor("L")    # set up left motor object
RightMotor = Motor("R")  # set up right motor object
RightMotor.stop()
LeftMotor.stop()

################
#  corridor()  #
################
def corridor():   # go down the corridor

    while True:                   # this version doesn't stop
        (left,front,right)= ReadWALLS()  # read all 3 walls
        target = 3500             # required left wall distance
        mult = 0.002              # steering strength
        
        # statements to end the loop (not used till step4)
        #if (left <1000):
        #    break                 # exit loop if no left wall
        #if (front >5000):
        #    break                 # exit loop if front wall
        
        error = left - target     # subtract target
        error = error * mult      # amount of steering
    
        lspeed = speed + error    # calsulate motor speeds
        rspeed = speed - error
        RightMotor.speed(rspeed)  # send speeds to motors
        LeftMotor.speed(lspeed)
        
        time.sleep(0.01)          # do a hundred readings per second


##########
#  main  #
##########
print(Thisfile)
print("Prints all 3 wall sensor values")

# await right pushbutton
while Button.value()==0: 
    #print("loop")
    count = 0    
    (left,front,right)= ReadWALLS()  # read all 3 walls
    print("L",left,"F",front,"R",right)
    
    time.sleep(0.1)
# await pushbutton release
while Button.value()==1:
    pass

led.on()

# run the corridor, then stop
corridor()
led.off()
RightMotor.stop()
LeftMotor.stop()