# Filename to be printed
Thisfile = "WKS9x.py"
# History
#14/12/2021 starter
# 11/1/2022 OldError
# 13/1/2022 Switches on button-press, derivative inclusion
# 26/1/22    All changes made in this file
#### 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
lspeed=0                   # speed of left motor
rspeed=0                   # speed of right motor
speed = 0                  # but see readswitches below
error = 0                  # calculated from sensors
OldError = 0               # used to track previous error
change =0                  # calculated from sensors
mult = 0.5                 # used for tuning
dfactor = 0                # used for tuning

#################
#  setspeeds()  #
#################
'''
 Function to calculate error and motor speeds from the sensor values 
'''
def setspeeds(ls,rs): # ls and rs are the sensor readings 
    global lspeed,rspeed   # Python to let this function change speeds
    global error,OldError,mult      # to allow changing error


    if (ls >20 or rs >20):
        led.off()
        error = ls-rs      # only if on-line,
    else:       
        led.on()           # Fall-off detected
        # Off-line, so boost error
        if error >0:
            error = 60
        else:
            error = -60
    val = error *mult      # adjust steering amount

    # Derivative calculations, using last-time's value
    change = (error - OldError)
    # add in the derivative to val
    val = val + (change * dfactor)/10

    lspeed = speed - val   # speed of left motor
    rspeed = speed + val   # speed of right motor
    OldError = error       # keep track of this error for later
    return (error)

##########
#  main  #
##########

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

print(Thisfile)

# pre race set-up to print the workings, and check everything
while Button.value()==0:    # wait for Start button button to be pressed
    lsense = calibl(lfront.read_u16()) # read left front sensor
    rsense = calibr(rfront.read_u16()) # read right front sensor
    
    setspeeds(lsense,rsense)  # set up error from readings
    
    print ("L ",lsense," R ",rsense,"Error",error)  # print the sensor values and error
    time.sleep(0.1)     # wait 100 milliseconds

swval = readswitches()   # read value of 4-way switch
if swval != 150 :  # ignore if ON,ON,ON,ON
    speed = swval
    pass
print("Speed="+str(speed))
	
# main loop to actually run the track
while True:
    lsense = calibl(lfront.read_u16()) # read left front sensor
    rsense = calibr(rfront.read_u16()) # read right front sensor
    
    setspeeds(lsense,rsense)  # set up speeds from readings

    LeftMotor.speed(lspeed)  # set left motor speed
    RightMotor.speed(rspeed) # set right motor speed

    time.sleep(0.01)        # wait 10 milliseconds

# end of loop
# never gets here