# test bluetooth
Thisfile = "DRAG1a.py"
# History
# 25/4/2022 pre_race() and race() made into functions
# 28/6/2022
#  UKMARSb used for reads()
#  reads() called instead of calibs() in awaitstop and race
#  speed set to 0 to stop. line 155


#### imports
from machine import Pin, PWM, ADC,UART
import time, utime
from UKMARS import *   # SW1, Button, lfront , rfront, Motor, calib[lrs](),reads()


speed = 45              # early speed  for 1/2 second
latespeed = 100         # speed after start (also set by switches )
mult = 0.1             # used for tuning  (0.3 is reasonable)
dfactor = 36             # used for tuning (5 is reasonable) 

FOError = 80            # Fall-off error (was 60)
FOlevel = 15            # Fall-off sensor value 
                        # larger number makes it fall  off sooner
# fall-off error dictates 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
############
#  logtxt  #
############
# record stuff to log file
def logtext():
    pfile = open("log.txt","a")
    pfile.write("\r\n\r\nspeed="+str(speed))
    pfile.write("\r\nmult="+str(mult))
    pfile.write("\r\ndfactor="+str(dfactor))
    pfile.write("\r\nloops="+str(loopcount)+" "+str(loopcount/160))
    pfile.close()
    print("log.txt written")


##################
#   awaitstop    #
##################

# Function to await white paper under stop sensor
# being inserted and removed
def awaitstop():
    ssense = reads(stopsense.read_u16()) # read stop sensor
    # await white paper
    while (ssense <60):
        ssense = reads(stopsense.read_u16()) # read stop sensor
        led.toggle()
        time.sleep(0.2)
    #await removal
    while (ssense >40):
        led.toggle()
        time.sleep(0.02)
        ssense = reads(stopsense.read_u16()) # read stop sensor
    led.off
    
#################
#  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 = speed * 0.8   # 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)

##############
#  pre_race  #
##############
def pre_race():
    # pre race set-up loop to calibrate sensors,
    #  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
     #   print ("Stop",ssense)  # print the sensor values and error
        time.sleep(0.1)     # wait 100 milliseconds
##########
#  race  #
##########
# function to run the Drag race
def race():
    global speed,mult,loopcount
    # main loop to actually run the track
    loopcount=0
    stoploop= 10000
    print("Start")
    while True:                      # Race LOOP - never stops
        if (loopcount == 80):         # 1/2 second
            speed = latespeed
            mult = .2

        lsense = calibl(lfront.read_u16())    # read left front sensor
        rsense = calibr(rfront.read_u16())    # read right front sensor
        ssense = reads(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.002)         # wait 5 milliseconds

        # stop on stop marker if >= 0.3  seconds after start
        if (ssense > 40 and loopcount >200):   # 0.6 seconds
            speed=0
            #RightMotor.stop()     #stop the motors just in case
            #LeftMotor.stop()                  # stop motors

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

print(Thisfile)
    
pre_race()      # pre-race loop, stops on push-button
swval = readswitches()   # read value of 4-way switch
if swval != 150 :  # ignore if ON,ON,ON,ON
    latespeed = swval*0.6 + 10
print("Latepeed="+str(latespeed))
print("Mult="+str(mult))


awaitstop()  # wait for white paper under stop sensor
race()   # race ends 3 seconds after stop

# end of program