# Filename to be printed
Thisfile = "DRAG1.py"    # from "WKS9z.py"
# History
# 17/4/2022

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

speed = 100                 # but see mainloop below
mult = 0.17                 # used for tuning  (0.3 is reasonable)
dfactor = 5                # used for tuning (5 is reasonable) 

FOError = 80            # Fall-off error (was 60)
FOlevel = 15            # Fall-off sensor value (was 20)calibl
                        # larger number makes it fall  off sooner
FOSpeed = 45            # 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
loopcount = 0

# Function to await white paper under stop sensor
# being inserted and removed
def awaitstop():
    ssense = calibs(stopsense.read_u16()) # read stop sensor
    # await white paper
    while (ssense <60):
        ssense = calibs(stopsense.read_u16()) # read stop sensor
        led.toggle()
        time.sleep(0.2)
    #await removal
    while (ssense >40):
        led.toggle()
        time.sleep(0.02)
        ssense = calibs(stopsense.read_u16()) # read stop sensor
    led.off
    
# 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 and 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
    ssense = calibs(stopsense.read_u16()) # read stop sensor
    
    setspeeds(lsense,rsense)  # set up error from readings
    
    print ("L ",lsense," R ",rsense,"Error",error,"Stop",ssense)  # 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*0.6 + 10

print("Speed="+str(speed))

awaitstop()
# main loop to actually run the track
loopcount=0
while True:
    if (loopcount == 1):      # start
        speed=50              # fast motors
        FOSpeed = 30          # Fall-off speed.

    if (loopcount == 80):      # early 0.5 seconds
        speed=30              # fast motors
        FOSpeed = 20          # Fall-off speed.

    lsense = calibl(lfront.read_u16())    # read left front sensor
    rsense = calibr(rfront.read_u16())    # read right front sensor
    ssense = calibr(stopsense.read_u16()) # read stop sensor
    
    setspeeds(lsense,rsense)  # set up speeds from readings

    LeftMotor.speed(lspeed)   # set left motor speed
    RightMotor.speed(rspeed)  # set right motor speed
     
    loopcount = loopcount +1
    time.sleep(0.005)         # wait 5 milliseconds

    # stop on stop marker
    if (ssense > 50 and loopcount >300):     # 1.5 seconds
        speed=0              # fast motors
        FOSpeed = 0          # Fall-off speed.            
    if (loopcount == 1000):  # 5 seconds
        break
# end of loop
# never gets here   
RightMotor.stop()     #stop the motors just in case
LeftMotor.stop()

print("END")