# Filename to be printed
Thisfile = "LFMk2a.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
# 9 feb Fall-off detection changed
#    Fall-off error changed from 60 -
#    Fall-off speed new
# 7/4/22 speed = swval/2  dfactor 25 mult 0.3 looptime .005
# fast start at 100 for 0.3 seconds
# 5/9/2022  speed adjustment on curveature
#  increases slowly to maxspeed if in centre of line
# instantly slows to minspeed if not in centre (see line 122)
# tuned for 660RPM motors

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

minspeed = 30  #(set  by switches line 109)
maxspeed = 90  #(NOT set  by switches line 110)
accell   = 2 # accelleration on the straight

speed = 0      # changed inside main loopo
mult = 0.3                 # used for tuning  (0.3 is reasonable)
dfactor = 20                # used for tuning (5 is reasonable) 

FOError = 70            # Fall-off error (was 60)
FOlevel = 30            # Fall-off sensor value (was 20)
                        # larger number makes it fall  off sooner
FOSpeed = 30            # Fall-off speed. (speed when fallen off)
# fall-off error and speed dictate curvature or steering back to line.

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

error = 0                  # calculated from sensors
OldError = 0               # used to track previous error
change =0                  # calculated from sensors

#################
#  setspeeds()  #
#################
'''
 Function to calculate error and motor speeds from the sensor values
 new for WKS9z
   detects fall-off at a tunable sensor level
   uses a tunable fall-off error value
   uses a tunable speed if fallen off
'''
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

    myspeed = speed

    if (ls >FOlevel or rs >FOlevel):  # detect not fallen off
        led.off()
        error = ls-rs      # only if on-line,
    else:       
        led.on()           # Fall-off detected
        # Off-line, so boost error
        if error >0:
            error = FOError
        else:
            error = -FOError
        #myspeed = FOSpeed   # run at fall-off speed
        
    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 = myspeed - val   # speed of left motor
    rspeed = myspeed + 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
    minspeed = swval
#    maxspeed = minspeed + 30

print("Speeds="+str(minspeed)+"  "+str(maxspeed))
	
# main loop to actually run the track
loopcount = 0
speed = minspeed
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
    if (error >35 or error <-35):
       speed = minspeed
    else:
       speed = min(speed+accell,maxspeed)

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

    time.sleep(0.005)        # wait 10 milliseconds
    loopcount = loopcount + 1

# end of loop
# never gets here