# Filename to be printed
Thisfile = "BASIC LF.py"
# History
# - uses Motor object
# - calibrate sensors
# - sensor fall-off code
# SWs can generate speed

#### imports
from machine import Pin, PWM, ADC,UART
import time, utime

from UKMARS import *   # SW1, Button, lfront , rfront, Motor

#############
#   Data    #
#############
from params import *  # fetch speed etc from params.py
# Tuned parameters in params.py:
#basespeed  = 0  # range 0 up to 100
#efactor    = 0  # efactor range 0-100
#dfactor    = 0  # 0-100
#Tlength    = 0  # wheelcount to stop at  (  1 = 94mm approx)
efactorc   = 100
dfactorc   = dfactor/10
state      = 0
oldsense = 0      # used to detect changes in error


# motors
LeftMotor  = Motor("L")    # set up left motor object
RightMotor = Motor("R")  # set up right motor object

#### start here ==========================================

print(Thisfile)

swval = SW1.value() *10
swval += SW2.value() *20
swval += SW3.value() *40
swval += SW4.value() *80
if (swval >100):
    swval = 99
    
if swval >0 :
    basespeed = swval
    
print("Speed="+str(basespeed))

RightMotor.stop()
LeftMotor.stop()

state = 0    # 0 = stopped  1 = running
error = 0
average = 0  # average error

loop = 0
while True:
        
    if Button.value()==1:      # Start button back left
        length=0
        while Button.value()==1:      # wait for Start button button released
            time.sleep(0.01)
            length=length+1
        #print("Length="+str(length))
        # invert state
        if (state == 1):
            state = 0
            led.off()
        else:
            state = 1
            led.on()

    olderr = error
    lsense = calibl(lfront.read_u16()) # read left front sensor
    rsense = calibr(rfront.read_u16()) # read right front sensor
#    if (state == 0):
#        print(str(efactor)+" "+str(int(average))+" "+str(error) + " " + str(lsense) + " " + str(rsense))
    
    if ((lsense > 15)and(rsense>15)):  
        error = lsense - rsense
        average = average *.7 + error
        eval = (error * efactor)/efactorc
        eval = eval + (error-olderr)*dfactorc
    else:
        if (average > 0):
            eval = 55 * efactor/efactorc
        else:
            eval = (0-55) * efactor/efactorc
        
    if (state == 1):
        bs = basespeed
        #if (SW1.value() == 1):    # switch 1 is on, slow if fallen off
        #    if ((error >30) or (error <-30)):
        #        bs = max((basespeed-5),0)
    else:
        bs = 0
        
                
#       
    lspeed = bs - eval
    rspeed = bs + eval
        
    if error > 0 :
        led2.off()
    if error < 0 :
        led2.on()
    loop = loop + 1
    if (loop>1000):
        loop = 0
        print(int(lsense)," ",int(rsense)," ",int(lspeed)," ",int(rspeed))
       
        
    LeftMotor.speed(int(lspeed)) # set left speed
    RightMotor.speed(int(rspeed)) # set right speed

    time.sleep(0.001)

# end of loop
LeftMotor.speed(0) # set left speed
RightMotor.speed(0) # set right speed