# Filename to be printed
Thisfile = "WKS32.py"
#Date  1/3/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.01            # amount of steering in corridor
target = 3000          # corridor left wall distance

lthresh = 1000         # Left wall present
lorun   = 0.25         # left turn overrun time (used later)
		
# 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:          # loop forever
        (left,front,right) = ReadWALLS()
		
        error = left - target     # subtract target
        error = error * mult      # amount of steering
    
        lspeed = speed + error
        rspeed = speed - error
        RightMotor.speed(rspeed)
        LeftMotor.speed(lspeed)
    
        time.sleep(0.01)
		
## Insert extra subroutines (functions) here

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

# await pushbutton press
while Button.value()==0: 
    (left,front,right) = ReadWALLS()
    print("L",left,"F",front,"R",right)    # display on Thonny
    time.sleep(0.1)
	
# await pushbutton release
while Button.value()==1:
    pass

led.on()

# Insert code here to loop forever and 
#  decide whether to turn left or run the next corridor


################################
# but we will start with always doing corridor 
corridor()
led.off()
RightMotor.stop()
LeftMotor.stop()

#(1/3/2022)