# Filename to be printed
Thisfile = "Tests.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)

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

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

print(Thisfile)



def waitB():
    while Button.value()==0:      # Start button back left
        lsense = calibl(lfront.read_u16()) # read left front sensor
        rsense = calibr(rfront.read_u16()) # read right front sensor
        pass
    while Button.value()==1:      # Start button back left
        lsense = calibl(lfront.read_u16()) # read left front sensor
        rsense = calibr(rfront.read_u16()) # read right front sensor
        pass
    print('Button')
    


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

RightMotor.stop()
LeftMotor.stop()

while True:
    LeftMotor.speed(0) # set left speed
    swval = SW1.value() *10
    swval += SW2.value() *20
    swval += SW3.value() *40
    swval += SW4.value() *80
    print("switches="+str(swval))
    lsense = calibl(lfront.read_u16()) # read left front sensor
    rsense = calibr(rfront.read_u16()) # read right front sensor
    print('lsense',lsense,'rsense',rsense)
        
    waitB()
    print('LED on')
    led.on()
    
    waitB()
    print('LED off')
    led.off()
    
    waitB()
    print('LED2 on')
    led2.on()
    
    waitB()
    print('LED2 off')
    led2.off()
        

    waitB()
    print('Left 20')
    LeftMotor.speed(20) # set left speed

    waitB()
    print('Left -20')
    LeftMotor.speed(-20) # set left speed

    waitB()
    LeftMotor.speed(0) # set left speed
    print('right 20')
    RightMotor.speed(20) # set left speed

    waitB()
    print('Right -20')
    RightMotor.speed(-20) # set left speed
    
    waitB()
    print('Right 0')
    RightMotor.speed(0) # set left speed
    
# end of loop
LeftMotor.speed(0) # set left speed
RightMotor.speed(0) # set right speed