# Filename to be printed
Thisfile = "WKS31.py"
#Date  5/12/2021

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

##########
#  data  #
##########
speed = 20             # speed in corridor
mult = 0.0015          # 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 = LWall.read_u16()
        front = FWall.read_u16()
        target = 30000            # required left wall distance
        mult = 0.0015             # steering strength
        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

def U_turn():   
    lspeed = 20      # adjust to suit
    rspeed = -20     # adjust to suit
    RightMotor.speed(rspeed)
    LeftMotor.speed(lspeed)
    time.sleep(0.5)  # adjsut to suit
    RightMotor.stop()
    LeftMotor.stop()
    time.sleep(0.1)

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

# await right pushbutton
while Button.value()==0: 
    #print("loop")
    count = 0    
    left = LWall.read_u16()
    front = FWall.read_u16()
    right = RWall.read_u16()
    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
while True:
    
    U_turn()
    
    time.sleep(1)