# Filename to be printed
Thisfile = "WKS9z2.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
# 9/3/2022 uses CheckStop and Clearstop to test stop sensor
#14/3/22 mult adjusted

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

speed = 80                  # but see readswitches below
mult = 0.25                 # used for tuning  (0.3 is reasonable)
dfactor = 25                # used for tuning (5 is reasonable) 

FOError = 60            # 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


    if (ls >FOlevel or rs >FOlevel):  # detect not fallen off
        led.off()
        error = ls-rs      # only if on-line,
        myspeed = speed
    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,"Markers",CheckStop())  # 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))

# Clear the stop marker count in UKMARS.py
ClearStop()

# 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.005)        # wait 5 milliseconds
    # only look for stop if on the line
    if (lsense >FOlevel or rsense >FOlevel):  # detect not fallen off
        if(CheckStop() == 2):
            speed =0
            FOSpeed = 0           # stop robot if 2nd marker seen

# end of loop
# never gets here