# Filename to be printed
Thisfile = "LFMk2d.py"
# REQUIRES UKMARS.py 28/4/22
# 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
# 18/5/22
# logfile function and stop marker detection added

#### 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
StopAt = 3                 # marker count to stop at

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

#############
# CountStop #
#############
markerCount = 0
overMarker = False

def CountStop():
    global overMarker,markerCount
    ssense = reads(stopsense.read_u16()) # read stop sensor    
    if (overMarker == True):  # if already over marker
        if (ssense <40):      # if no longer over marker
            overMarker = False
    else:
        if (ssense >60):      # if over marker
            markerCount = markerCount+1
            print (markerCount)
            overMarker = True
    return(markerCount)

	


##########
#  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
    CountStop()                 # check stop marker
    
    print ("L ",lsense," R ",rsense,"Error",error,markerCount)  # 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
written=0  # whether logfile has been written
markerCount = 0
overMarker = False
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

    CountStop()                 # check stop marker
    if (speed != 0):
        if (error >35 or error <-35):
            if (maxspeed - speed < 20):
                LeftMotor.speed(-20)  # set left motor speed
                RightMotor.speed(-20) # set right motor speed
                #time.sleep(0.01)        # wait 5 milliseconds

            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 6 milliseconds
    loopcount = loopcount + 1

    # stop on stop marker if StopAt markers reached
    if (markerCount >= StopAt):
        speed = 0             # stop motors
        if(written == 0):
            logtext()
            written=1
        
    

# end of loop
# never gets here