# Filename to be printed
Thisfile = "WKS32z.py"
#Date  2/3/2022
# all modifications from Worksheet 32 added
# (un-adjusted)

#############
#  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)
        if left <lthresh:     # if left wall missing
            RightMotor.speed(35) # Go straight ahead adjust speed
            LeftMotor.speed(35)
            time.sleep(lorun)    # adjust lorun to get distance right
            RightMotor.speed(0)  # stop robot
            LeftMotor.speed(0)
            return               # exit corridor		
## Insert extra subroutines (functions) here
################
#  TurnLeft()  #
################
def TurnLeft():   # gently turn left

    RightMotor.speed(30)     # Right motor faster
    LeftMotor.speed(10)      # than left motor
    time.sleep(0.6)          # adjust time to get 90 degree turn

##########
#  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

# inside main loop
while True:
    (left,front,right)= ReadWALLS()  # read all 3 walls
    if (left <lthresh):   #  if no left wall
        TurnLeft()
        RightMotor.speed(0)  # Stop after each turn
        LeftMotor.speed(0)   # 
        time.sleep(1)        # for 1 second

    else:                    # there is a left wall
        corridor()           # run the corridor, then stop

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

#(1/3/2022)