# Filename to be printed
Thisfile = "WKS8.py"
# History
# speed 20
#23/11/2021 simpler Python


#### imports
from machine import Pin, PWM, ADC,UART
import time, utime
from UKMARS import *   # SW1, Button, lfront , rfront, Motor

#############
#   Data    #
#############
# motors
LeftMotor  = Motor("L")    # set up left motor object
RightMotor = Motor("R")    # set up right motor object
zone = 1                   # used to say what zone we are in
                           # zone 3 is the middle, zone 1 is far right
                           # zone 5 is far left etc
lspeed=0                   # speed of left motor
rspeed=0                   # speed of right motor


#======== Program starts here =======================
print(Thisfile)

RightMotor.stop()     #stop the motors just in case
LeftMotor.stop()

##################
#   setspeeds    #
##################
# this function sets the motor speeds from the sensor reading
def setspeeds(val): # val is the input (=rsense reading)
    global zone,lspeed,rspeed   #allows this function to set zone and speeds
    if val < 20:
        zone= 1
        lspeed=18  # adjust to suit
        rspeed=22  # adjust to suit
    elif val < 40:
        zone= 2
        lspeed=19  # adjust to suit
        rspeed=21  # adjust to suit
    elif val < 60:
        zone= 3
        lspeed=20  # adjust to suit
        rspeed=20  # adjust to suit
    elif val < 80:
        zone= 4
        lspeed=21  # adjust to suit
        rspeed=19  # adjust to suit
    # otherwise
    else:
        zone= 5
        lspeed=22  # adjust to suit
        rspeed=18  # adjust to suit

# pre race set-up to print the workings, and check everything
while Button.value()==0:    # wait for Start button button to be pressed
    rsense = calibr(rfront.read_u16()) # read right front sensor
    setspeeds(rsense)       # sets up motor speeds from rsense
    print ("Zone",zone," L ",lspeed," R ",rspeed)  # print the sone and speeds
    time.sleep(0.1)

# main loop to actually run the track
while True:
    rsense = calibr(rfront.read_u16()) # read right front sensor
    setspeeds(rsense)      # sets up motor speeds from rsense
    LeftMotor.speed(lspeed)  # set left speed
    RightMotor.speed(rspeed) # set right speed

    time.sleep(0.001)

# end of loop
# never gets here