# WHAM_LF  20/10/2022
Thisfile = "WHAM_LF.py"
#############
#  imports  #
#############
from machine import Pin, PWM, ADC
import time
import utime
from WHAM import *

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


# pre-race
intmax = 0
intmin = 65535
Speed = 0
Efactor = 5
Dfactor = 20

while True:
    Speed =  SW1.value()
    Speed += SW2.value()*2
    Speed += SW3.value()*4
    Speed += SW4.value()*8
    Speed = Speed *6

    val = IntSense.read_u16()
    intmax = max(intmax,val)
    intmin = min(intmin,val)
    if (intmax-intmin) != 0:
        val = int((val-intmin)*100/(intmax-intmin))
        print (val,Speed)
    if val<20:
        L_led.off()
        M_led.off()
        R_led.off()
    elif val<40:
        L_led.on()
        M_led.off()
        R_led.off()
    elif val<60:
        L_led.off()
        M_led.on()
        R_led.off()
    elif val<80:
        L_led.off()
        M_led.off()
        R_led.on()
    
    else:
        L_led.off()
        M_led.off()
        R_led.off()
    if (Button3.value()==1):
        break
    time.sleep(.1)
    
print ("Stopped")

while (Button3.value()==1):
    time.sleep(.1)
    
    oldval = 50
while True:
    val = IntSense.read_u16()
    val = int((val-intmin)*100/(intmax-intmin))
    sdiff = (val-50) * Efactor / 10
    sdiff = sdiff + (val-oldval)*Dfactor/10
    lspeed = Speed - sdiff
    rspeed = Speed + sdiff
    oldval = val
    
    print (val,lspeed,rspeed)
    
    
    LeftMotor.speed(lspeed)  # set left motor speed
    RightMotor.speed(rspeed) # set right motor speed

    time.sleep(0.01)
    if (Button3.value()==1):
        break
    
print("Stopped")
LeftMotor.speed(0)  # set left motor speed
RightMotor.speed(0) # set right motor speed