# 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=1                   # speed of left motor
rspeed=1                   # speed of right motor
basespeed = 30
 
#======== 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
#    multiplier = 2
#    basespeed = 20
    
 
# 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
    if rsense < 1:
        rsense = 1
    elif rsense > 99:
        rsense = 99
    error = 50 / rsense
    lspeedcomp = (lspeed * basespeed) * error
    rspeedcomp = (rspeed * basespeed) / error
    
    if lspeedcomp > 30:
        lspeedcomp = 30
    if rspeedcomp < 10:
        rspeedcomp = 10
    
    LeftMotor.speed(int(lspeedcomp))  # set left speed
    RightMotor.speed(int(rspeedcomp)) # set right speed
    print("lspeed:", lspeedcomp, rspeedcomp)
    #print(rsense, error)
 
    time.sleep(0.1)
 
# end of loop
# never gets here