# june17C4PO.py  8/6/2025 Mr Fisher
# tuned for R4D4
#  leftthres/3 to strt left turn
# Calibration function changed, and now used
#  it uses stored values if SW4 is on, else reads new values
#  and stores them
# Screen messages improved.
# Tuning still needed
# 11 June moves() function

moneyForNothing_Remastered1996_ = False
screen = True
lifted = False
'''  incorporated...
  (1) "#" to run auto
  (2) "0" to stop. This needs to be put outside cb()
        trying readWalls()
  (3) check globals Mr Robinson
  (4) TOBY updated reduced to 61
  (5) leftThresh/2 used as threshold for left turn
  (6) "U" and "D" for up arrow/downarrow
  (7) 
  (8) L,R,C decision order not R,L,C
  (9) left turn 0.8  1.25 motor speeds
  (10) corridor() actually called at end
  (11) decode dictionary

# Not yet done....
  (7) Emily, corridor and turnleft to write log
  (12) linear wall sensors
  (13) keep count of runs. Stop run by lifting robot out of maze
  (14) BOH on 1st run
  (15) Read Run 1 log to find other BOHs
  (16) TUNING section (incl right motor tuning )
  (17) FT from front threshold during calibrate
'''
#-----------------------------------------
from machine import Pin, Timer, PWM, ADC, I2C,UART
import utime, time
from ssd1306 import SSD1306_I2C

SW1 = Pin(0, Pin.IN, Pin.PULL_UP)       #Hw1 1st switch +++
SW2 = Pin(1, Pin.IN, Pin.PULL_UP)       #Hw2 2nd switch +++
SW3 = Pin(2, Pin.IN, Pin.PULL_UP)       #Hw4 3rd switch +++
SW4 = Pin(3, Pin.IN, Pin.PULL_UP)
SWVAL = SW1.value()+2*SW2.value()+4*SW3.value()
tuningNum = SWVAL
if screen:
    i2c=I2C(1,sda=Pin(18),scl=Pin(19),freq=200000)
    dsp=SSD1306_I2C(128,32,i2c)#,48
    dsp.fill(0)
else:
    print("SWVAL",SWVAL)
    time.sleep(1)
# Bluetooth serial potr
uart = UART(0, baudrate=9600, tx=Pin(16), rx=Pin(17), bits=8, parity=None, stop=1) # +++

from ir_rx.print_error import print_error  # Optional print of error codes

# Import IR reception class for mini- remote
from ir_rx.nec import NEC_8
p = Pin(20, Pin.IN)

# Mini NEC-8 
decode = {
    69: "1", 70: "2", 71: "3",
    68: "4", 64: "5", 67: "6",
    7:  "7", 21: "8",  9: "9",
    22: "*", 25: "0", 13: "#", 
    
    24: "U",                   #       UP
    8: "L",  28: "O", 90: "R", # Left, OK, Right
    82: "D",                   #      Down
}

##############################
#####                     ####
#####       cb()          ####
#####                     ####
##############################
tr="?"
last_cb = 0

# call-back from IR receiver interrupt:
def cb(data, addr, ctrl):
    global last_cb,tr
    tr="?"
    time_since = time.ticks_diff(time.ticks_ms(), last_cb)
    last_cb = time.ticks_ms()
    if time_since < 500:  # NEC protocol sends repeat codes.
        #print(".",end="")
        pass
    
    else:
        #print(f"Data = {data}")
        try:
            tr = decode.get(data,)  # 
        except:
            tr = "?"
        #if (tr == "0"):  #stop  (2)
        #    st()
        if (tr == "?"):  # unknown
            print(tr, data)
            if screen:
                dsp.fill(0)
                dsp.text("code "+ str(data),0,0)
                dsp.text(str(tr),0,10)
                dsp.show()
        else :
            print(tr,end="")
            if tr == "P": print("")
            if screen:
                dsp.fill(0)
                dsp.text("code "+ str(data),0,0)
                dsp.text(str(tr),0,10)
                dsp.show()


    

ir = NEC_8(p,cb)
ir.error_function(print_error)  # Show debug information


if screen:
    i2c=I2C(1,sda=Pin(18),scl=Pin(19),freq=200000)
    dsp=SSD1306_I2C(128,32,i2c)#,48
    dsp.fill(0)
    
def BT(strg):
    uart.write(bytes(strg,"utf8"))
    print("BT:"+strg)

Olderr = 0

lwheel = Pin(6,Pin.IN)
rwheel = Pin(7,Pin.IN)
tst = Pin(14, Pin.OUT) #transistor
led_onboard = Pin(25, Pin.OUT)
led = Pin(25, Pin.OUT)
ledL = Pin(13, Pin.OUT)
ledR = Pin(8, Pin.OUT)
Lsensor = ADC(27)
Msensor = ADC(26)
Rsensor = ADC(28)
button = Pin(22,Pin.IN, Pin.PULL_DOWN)


left_dir = Pin(9, Pin.OUT)
right_dir = Pin(10, Pin.OUT)
left_pwm = PWM(Pin(11))
right_pwm = PWM(Pin(12))
left_pwm.freq(1000)
right_pwm.freq(1000)
LDistance = 0
RDistance = 0
Distance = 0
# TUNING
Lstp = 0.5
Fstp = 0.5
FT = 10000

##############################
#####                     ####
#####      tune()         ####
#####                     ####
##############################
# new 2/5/25 Mr Fisher.
# can respond to switches or TV remote
# takes argument 0 - 4 to choose tuning

def tune(number):
    global motorcomp, SPspeed, TOBY
    global mult, Kd, speed, LSPD, RSPD
    global velociraptor_mongoliensis
    global manospondylus_gigas,tuningNum,Lstp,Fstp
    tuningNum = number
    
    print("tune("+str(number)+")")
    if (number == "0"): number = 0
    if (number == "1"): number = 1
    if (number == "2"): number = 2
    if (number == "3"): number = 3
    if (number == "4"): number = 4
    
    motorcomp = 1.075# motor compensation
    SPspeed = 8000     # right spin speed
     # (4) left wheel encoder 90 degree spin   #TUNING
    # FT = 9000          # dont tune front threshold. see calibratoin
    # spin
# tune 0
    if number == 0:
        mult = 2
        Kd = 2
        speed = 8000
        LSPD = 0.8
        RSPD = 2
        velociraptor_mongoliensis = 80 # FWD before Left
        manospondylus_gigas = 10        # kink Right
        TOBY = 62   #was 64                      # Spin right
        Lstp = 0.5
        Fstp = 1
        
    elif number == 1:
        mult = 2
        Kd = 2
        speed = 10000
        LSPD = 0.75
        RSPD = 1.75
        velociraptor_mongoliensis = 90
        manospondylus_gigas = 10
        TOBY =  62 #was 64                      # Spin right
        Lstp =0.5
        Fstp = 1
        
    elif number == 2:
        mult = 2.5 #1
        Kd = 2
        speed = 12500
        LSPD = 0.75
        RSPD = 2
        velociraptor_mongoliensis = 65
        manospondylus_gigas = 10              #kink right
        TOBY =  62 #was 72                      # Spin right
        Lstp =0.5
        Fstp = 0.8

    else:       # number = 3 or more
        mult = 3 #1   
        Kd = 2
        speed = 15000
        LSPD = 0.75
        RSPD = 2
        velociraptor_mongoliensis = 40
        manospondylus_gigas = 5                 # was 10 kink
        TOBY =  62 #was 70                      # Spin right
        Lstp = 0.5
        Fstp = 0.7   # stop on smaller front sensor
        
    if screen:
        dsp.fill(0)
        dsp.text(str(number),100,0)
        dsp.text("Sp "+ str(speed),0,0)
        dsp.text("Kp "+ str(mult)+" Kd "+ str(Kd),0,10)
        dsp.text("Ls "+ str(LSPD)+" Rs "+ str(RSPD),0,20)
        dsp.show()
        time.sleep(0.4)
        BT("TU"+str(number))
tune(SWVAL)
print("Speed",speed)

R2 = 1#right turn
D2 = 2#U turn
C3 = 3#left turn
PO = 4#corridor

##############################
# #####                     ####
#####     motors()        ####
#####                     ####
##############################
def motors(left,right):
    #print("motors("+str(left)+","+str(right)+")")
    right *= motorcomp #1.103    #TUNING
    if left<0:
        left = -left
        left_dir.on()
    else:
        left_dir.off()
    if right<0:
        right = -right
        right_dir.on()
    else:
        right_dir.off()
    
    left  = min(left,65001)
    right = min(right,65001)
    left_pwm.duty_u16(int(left)) 
    right_pwm.duty_u16(int(right)) 

##############################
#####                     ####
#####    clearLOG()       ####
#####                     ####
##############################
def clearLOG():
    f = open("log.txt", "w")
    f.close()
    if screen:
        dsp.fill(0)
        dsp.text("log gone, never ",0,0)
        dsp.text("to return",0,10)
        dsp.show()
    time.sleep(1)
    
def writeLOG(msg):
    print("msg",msg)
    f = open("log.txt", "a")
    f.write(msg + "\n")
    f.close()
    
def jotLOG(msg):
    print("mr clapson",msg)
    f = open("log.txt", "a")
    f.write(str (msg))
    f.close()
    
    

if screen:
    dsp.fill(0)
    dsp.text("speed:"+str(speed),0,0)
    dsp.text("FT: ???",0,10)
    dsp.text("mult:"+str(mult),0,20)
    dsp.show()
time.sleep(1)

rwd = .001     #TUNING
def tobynotgood():
    motors(-speed,-speed)#reverse
    time.sleep(1)
    stop()
    
def malcolm(dist):
    stopat = LDistance + RDistance + dist
    motors(speed,speed)
    while (LDistance + RDistance) < stopat:
        pass
    stop()

def Mr_Fisher(angle):
    oldAngle=LDistance - RDistance
    
    BT("MF"+str(angle)+" ")  # Bluetooth
    
    #while (LDistance - RDistance)< (angle + oldAngle):
    motors(10000,-10000)
    time.sleep(angle/200)
    stop()


oldState = None
def Mr_Robinson(newState):
    return
    global oldState, Distance, LDistance, RDistance  #(3)
    if oldState == None:
        oldState = newState
        jotLOG(newState)#start line in log
    else:
        if oldState != newState:
            writeLOG(" "+str( LDistance + RDistance - Distance ) )
            jotLOG(newState)
            Distance = LDistance + RDistance
    
##############################
#####                     ####
#####   Lwheelirq()       ####
#####                     ####
##############################
def Lwheelirq(which):
    global LDistance,IR            # count of wheel markers
    IR= "L"
    #if intenb:
    LDistance += 1              # increase count

# Setup for left wheel sensor
def Rwheelirq(which):
    global RDistance,IR            # count of wheel markers
    IR= "R"
    #if intenb:
    RDistance += 1# increase count

# set up interrupt handler for each pin
lwheel.irq(handler=Lwheelirq,trigger=Pin.IRQ_FALLING|Pin.IRQ_RISING) 
rwheel.irq(handler=Rwheelirq,trigger=Pin.IRQ_FALLING|Pin.IRQ_RISING)#while True:
#    led.value(button.value())
#while True:
 #   ledL.value(lwheel.value())
  #  ledR.value(rwheel.value())
   # print(LDistance,RDistance)
    #time.sleep(1)

    

def stop():
    motors(0,0)

def st():
    stop()
    1/0
##############################
#####                     ####
#####   readWalls()       ####
#####                     ####
##############################
def readWalls():
    global lifted
    if (tr == "0"):  #stop  (2)
        print("STOP")
        st()
    tst.value(0)
    time.sleep(rwd)
    LMin = Lsensor.read_u16()
    MMin = Msensor.read_u16()
    RMin = Rsensor.read_u16()

    tst.value(1)
    time.sleep(rwd)
    MMax = Msensor.read_u16()
    RMax = Rsensor.read_u16()
    LMax = Lsensor.read_u16()

    tst.value(0)
    Lside = LMax - LMin
    Mside = MMax - MMin
    Rside = RMax - RMin

    if (Lside <400) & (Mside <400) & (Rside <400 ):
        if lifted== False: print("Lift"); BT("Lft")
        lifted = True
        if screen:
            dsp.fill(0)
            dsp.text("Lifted  "+str(tuningNum), 0,0)
            dsp.show()

    else:
        lifted = False
        
    return Lside, Mside, Rside

##############################
#####                     ####
#####   spinRight90()     ####
#####                     ####
##############################    
def spinRight90():
    global LDistance
    Fullspeed = True
    #LDistance = 0
    x = LDistance+TOBY
    #dsp.fill(0)
    #dsp.text("turning right", 0,0)
    #dsp.show()
    #right_dir.on()     # reverse right motor
    #left_dir.off()     # forward left motor
    #left_pwm.duty_u16(speed)   #reset
    #right_pwm.duty_u16(int(speed*1.2))
    motors(SPspeed,-SPspeed*1.2)
    # slow down early...
    while LDistance<x:
        if Fullspeed and LDistance>x-10:           #TUNING
            Fullspeed= False
            motors(SPspeed-1000,-(SPspeed-1000))    #TUNING
        pass
    motors(0,0)
    print("EndRSp")

    
    
    
##############################
#####                     ####
#####     turnLeft()      ####
#####                     ####
##############################

def turnLeft():
    BT("TL ")  # Bluetooth
    ledL.on()
    (l,f,r) = readWalls()
    # spin a bit
    Mr_Fisher(manospondylus_gigas)
    malcolm(velociraptor_mongoliensis)
    if screen:
        dsp.fill(0)
        dsp.text("turning left", 0,0)
        dsp.show()
    motors(speed*LSPD,speed*RSPD)
    while True:
        (l,f,r) = readWalls()
        if lifted:
            stop()
            print("LSTP lift"+str(l))
            return
        if l > leftThresh*Lstp : #"and  r < rightThresh"
            print("LSTP L="+str(l))
            break
        
    ledL.off()
    stop()
    BT("TLx ")  # Bluetooth    
##############################
#####                     ####
#####       BOH()         ####
#####                     ####
##############################
def BOH():
    print("bat out of hell")
    speed2 = speed + 2000
    #speed2 = 15000
    (l,f,r) = readWalls()
    KD = 60
    
    while True:
        Olderr = 0
        (l,f,r) = readWalls()
        error = l - leftThresh
        error = error*mult*(speed2/speed)
        deriv = (error - Olderr)*KD
        sDiff = error + deriv
        Olderr = error
        lSpeed = max(speed2+sDiff,0)
        lSpeed = min(lSpeed,65000)
        rSpeed = max(speed2-sDiff,0)
        rSpeed = min(rSpeed,65000)
        (l,f,r) = readWalls()
        #left_pwm.duty_u16(int(lSpeed))   #reset
        #right_pwm.duty_u16(int(rSpeed))
        motors(lSpeed,rSpeed)
            
        if f > FT/4.5:
            print(f)
            writeLOG("BOH ")
            stop()
            return
          

##############################
#####                     ####
#####    calibrate()      ####
#####                     ####
##############################
# returns 3 wall values
# if SW4 is on, it returns stored values
def calibrate():
    global FT
    count=1
    if (SW4.value() == 1):
        try:
            import calib
            if screen:
                dsp.fill(0)
                dsp.text("Saved calib",0,0)
                dsp.show()
                time.sleep(1)
            return( (calib.leftThresh,calib.frontThresh,calib.rightThresh) )
        except:
            pass
    # read averaged values from walls
    dsp.fill(0)
    dsp.text("Calibrating 99",0,0)
    dsp.text("SW4"+str(SW4.value()),0,10)
    dsp.show()
    
    x=1
    (leftThresh,frontThresh,rightThresh) = (0,0,0)
    AverageAmount = 0.8   # amount of old reading
    NewAmount = 1-AverageAmount  # amount of new reading
    print("Calibrating")
    BT("Calib ")  # Bluetooth
    while button.value() == 0:
        count+=1
        (l,f,r)=readWalls()
        leftThresh  = (leftThresh*AverageAmount)  + (l*NewAmount)
        frontThresh = (frontThresh*AverageAmount) + (f*NewAmount)
        rightThresh = (rightThresh*AverageAmount) + (r*NewAmount)
        leftThresh  = int(leftThresh)
        frontThresh  = int(frontThresh)
        rightThresh  = int(rightThresh)
        print(leftThresh,frontThresh,rightThresh,LDistance)
        if leftThresh > 6000:
            ledL.on()
        else:
            ledL.off()
        if rightThresh > 6000:
            ledR.on()
        else:
            ledR.off()
        if frontThresh > 10000:
            led.on()
        else:
            led.off()
        #frontThresh = FT
        FT = frontThresh  #(17)
        if count>5:
            count=0
            if screen:
                dsp.fill(0)
                dsp.text("Calib",0,0)
                dsp.text(str(leftThresh)+"    " +str(rightThresh),0,10)
                dsp.text("FT: "+str(frontThresh),0,20)
                dsp.show()

        time.sleep(0.1)
    # end of calibration

    # save readings to file
    BT("Cs ")  # Bluetooth
    print("Saving")
    f = open("calib.py", "w")
    f.write("leftThresh  = " + str(leftThresh) + "\n")
    f.write("frontThresh = " + str(frontThresh)+ "\n")
    f.write("rightThresh = " + str(rightThresh)+ "\n")
    f.close()
    print("Saved")
    if screen:
        dsp.fill(0)
        dsp.text("Live calib",0,0)
        dsp.show()
    return( (leftThresh,frontThresh,rightThresh) )

        
# Main program strts here .......

##############################
#####                     ####
#####         MAIN        ####
#####                     ####
##############################
#main loop
stop()
led.off()

### OLD Calibration code.....
'''
if screen:
    dsp.fill(0)
    dsp.text("Calibrating",0,0)
    dsp.show()
    BT("\nCalib ")  # Bluetooth
while True:
    (leftThresh,frontThresh,rightThresh)=readWalls()
    print(leftThresh,frontThresh,rightThresh)
    if leftThresh > 7000:
        ledL.on()
    else:
        ledL.off()
    if rightThresh > 6000:
        ledR.on()
    else:
        ledR.off()
    if frontThresh > FT:
        led.on()
    else:
        led.off()
    #frontThresh = FT
    FT = frontThresh  #(17)

    time.sleep(0.1)
    if button.value() == 1:
        break
#print("BP1")

BT("Cx "+str(leftThresh)+" "+str(frontThresh)+" "+str(rightThresh))  # Bluetooth

while button.value() == 1:
    pass
print("BR1")
'''

(leftThresh,frontThresh,rightThresh) = calibrate()
FT = frontThresh  #(17)

if screen:
    dsp.fill(0)
    dsp.text("Calib",0,0)
    dsp.text(str(leftThresh)+"    " +str(rightThresh),0,10)
    dsp.text("FT: "+str(frontThresh),0,20)
    dsp.show()
    time.sleep(2)

# Await hand wave
    dsp.fill(0)
    dsp.text("Wave hand",0,0)
    dsp.show()
(l,f,r) = readWalls()

while f < 1500:
    (l,f,r) = readWalls()
    print(f)
    time.sleep(0.1)

led_onboard.on()

while f > 2000:
    (l,f,r) = readWalls()
    print(f)
    time.sleep(0.1)
led_onboard.off()

### END OF OLD Calibration code.....


##############################
#####                     ####
#####     corridor()      ####
#####                     ####
##############################
def corridor():
    global tr,  moneyForNothing_Remastered1996_,Olderr
    MySpeed = speed
    Olderr = 0
    if moneyForNothing_Remastered1996_ :
        MySpeed = speed + 2000
        moneyForNothing_Remastered1996_ = False
    
        dsp.fill(0)
        dsp.text("BOH", 0,0)
        dsp.show()
    
    print("corridor")
    BT("Cor ")  # Bluetooth
    while True:
        if (tr == "1") or (tr == "2") or (tr == "3"):
            stop()
            tune (tr)
            tr = "?"
        global LDistance, RDistance, l, Distance, Olderr, OldL
        (l,f,r) = readWalls() # checking for a wall ahead, or missing left wall
        if f >= FT*Fstp :
#            
            stop()
            if screen:
                dsp.fill(0)
                dsp.text("Front "+ str(f),0,0)
                dsp.show()
            BT("Cx ")  # Bluetooth    
            return
        if l < leftThresh/3:
            stop()
            if screen:
                dsp.fill(0)
                dsp.text("Left "+ str(l),0,0)
                dsp.show()
            BT("Cx ")  # Bluetooth    
            return
        time.sleep(0.01)
        Distance = LDistance + RDistance
        error = l - leftThresh
        deriv = (error - Olderr)*Kd
        sDiff = error + deriv
        sDiff = sDiff*mult
        Olderr = error
        lSpeed = max(MySpeed+sDiff,0)
        #lSpeed = min(lSpeed,65000)
        rSpeed = max(MySpeed-sDiff,0)
        #rSpeed = min(rSpeed,65000)
        #left_pwm.duty_u16(int(lSpeed))   #reset
        #right_pwm.duty_u16(int(rSpeed))
        motors(lSpeed,rSpeed)
        oldL = l
        
        
       
##############################
#####                     ####
#####       emily()       ####
#####                     ####
##############################
def emily():
    (l,f,r) = readWalls()
    print("emily")
    BT(" Em")  # Bluetooth
    ledR.on()
    if l > 0: #(leftThresh - 1000):  # why depend on left sensor to spin right??
        led_onboard.off()
        print("stp2")
        spinRight90()
        spincount = 1
        stop()

        (l,f,r) = readWalls()
        if f > FT/1.5 :
            spinRight90()
            spincount = 2
        if spincount == 1:
            #toby()
            pass
    # no right spin, or after right spin    
    #(l,f,r) = readWalls()
   
    #while l > leftThresh:
    #    motors(speed,-speed)
    #    (l,f,r) = readWalls()
    stop()
    ledR.off()
    BT("Ex ")  # Bluetooth    




def moves(count):
    print("FT+"+str(FT))
    while(count>0):
        print("Mv"+str(count))
        (l,f,r) = readWalls()
        if l < leftThresh/2:  # sudden drop in left wall
            turnLeft()
        else:
            if f > FT/2:      # start right spin
                emily()
            else:
                corridor()
        count -= 1
      

# new MRF-------

if screen:
    dsp.fill(0)
    dsp.text("TVR to start",0,0)
    dsp.text(str(tr),0,10)
    dsp.show()

while True:
    if tr != "?":
        if tr == "O":
            (l,f,r) = readWalls()
            emily()
            
        if tr == "L":
            turnLeft()
            
        if (tr == "1") or (tr == "2") or (tr == "3"):
            tune(tr)
            
        if (tr == "4") or (tr == "5") or (tr == "6"):           
            moves(int(tr)-3)
            
        if tr == "7":
            motors(10000,10000)
            time.sleep(0.5)
            stop()
            
              
                
             
        if tr == "8":
            malcolm(velociraptor_mongoliensis)
            if screen:
                dsp.fill(0)
                dsp.text("velo "+ str(velociraptor_mongoliensis),0,0)
                dsp.show()
                
        if tr == "9":
            Mr_Fisher(manospondylus_gigas)
            if screen:
                dsp.fill(0)
                dsp.text("Mano "+ str(manospondylus_gigas),0,0)
                dsp.show()
            
        if tr == "R":
            spinRight90()
        if (tr == "v")|(tr=="D"): #(6)
            spinRight90()
            spinRight90()
        if (tr == "^")|(tr=="U"):  # (6)
            corridor()
        if tr == "#":  # (1)
            break
        if tr == "*":
            moneyForNothing_Remastered1996_ = True
        tr = "?"
        print(".")
        time.sleep(0.01)
        
            
        
oldL = 0
tr = "?"   # clear TV remote commands
while True:
    # check for TVR and re-tune if necessary
    # if tr == "1"  etc.etc.
    # record start of run in logstring
    while True:
        # Or put re-tuning in here...
        if lifted: break
        (l,f,r) = readWalls()
        if l < leftThresh/2:  # sudden drop in left wall
     #    if (l - oldL)<-1000:  # sudden drop in left wall
            print(l,oldL)
            Mr_Robinson(C3)
           
            turnLeft()
            ledL.off()
            
        if f > FT/2:			# start right spin
            emily()
        else:
            corridor()
            #--------- Corridor steering ------------
            Mr_Robinson(PO)
        if lifted:
            stop()
            BT("Liftx")
            break    # end of run if lifted
    BT("Outer")
    stop()
    if screen:
        dsp.fill(0)
        dsp.text("   LIFTED  "+str(tuningNum),0,0)
        dsp.text("button for",0,10)
        dsp.text("  next run",0,20)
        dsp.show()

    while button.value()==0:
        if (tr == "1") or (tr == "2") or (tr == "3"):
            tune(tr)
            if screen:
                dsp.fill(0)
                dsp.text(tr,100,0)
                dsp.text("Sp "+ str(speed),0,0)
                dsp.text("Kp "+ str(mult)+" Kd "+ str(Kd),0,10)
                dsp.text("Ls "+ str(LSPD)+" Rs "+ str(RSPD),0,20)
                dsp.show()
        tr="?"
        pass
    lifted = False
    #st()
    # write logstring to log file

#left_pwm.duty_u16(0)   #reset
#right_pwm.duty_u16(0)
motors(0,0)