# May2.py Tune() function added
# From Zach version 14/3/2025 #april26.py
# Tune function used only for SWVAL not TVR (yet)
# 2/5/2025
#  TVR tuning added in corridor
# stop before right turn on tuning 1 setting but not 2
# calibrate function added but not used.
# 5/5/24 Mr Fisher  Bluetootg logging added

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) RightspinEnc 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()+8*SW4.value()
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

##############################
#####                     ####
#####      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, FT,RightspinEnc
    global mult, Kd, speed, LSPD, RSPD
    global velociraptor_mongoliensis
    global manospondylus_gigas
    
    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.103  # motor compensation
    SPspeed = 8000     # right spin speed
    RightspinEnc = 67 # (4) left wheel encoder 90 degree spin   #TUNING
    FT = 9000          # front threshold
    if number == 0:
        mult = 1.2     # corridor steering
        Kd = 2         # derivative
        speed = 6000   # corridor speed
        LSPD = 0.8     # Left turn left motor
        RSPD = 2       # Left turn right motor
        velociraptor_mongoliensis = 60   # forward
        manospondylus_gigas = 8        # spin
        
        
    elif number == 1:
        mult = 2
        Kd = 2
        speed = 10000
        LSPD = 0.8
        RSPD = 2
        velociraptor_mongoliensis = 50
        manospondylus_gigas = 10
        
    elif number == 2:
        mult = 2.5 #1
        Kd = 2
        speed = 12500
        LSPD = 0.87
        RSPD = 2
        velociraptor_mongoliensis = 40
        manospondylus_gigas = 10
    else:       # number = 3 or more
        mult = 3 #1
        Kd = 2
        speed = 15000
        LSPD = 0.87
        RSPD = 2
        velociraptor_mongoliensis = 30
        manospondylus_gigas = 10
    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()
        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 SW4.value() == 1:
    clearLOG()
    

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



rwd = .001     #TUNING
def toby():
    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", 0,0)
            dsp.show()

    else:
        lifted = False
        
    return Lside, Mside, Rside

##############################
#####                     ####
#####   spinRight90()     ####
#####                     ####
##############################    
def spinRight90():
    global LDistance
    Fullspeed = True
    #LDistance = 0
    x = LDistance+RightspinEnc
    #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)
    # 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();return
        if l > leftThresh*0.5: #"and  r < rightThresh"
            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
def calibrate():
    (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 True:
        (l,f,r)=readWalls()
        leftThresh  = (leftThresh*AverageAmount)  + (l*NewAmount)
        frontThresh = (frontThresh*AverageAmount) + (f*NewAmount)
        rightThresh = (rightThresh*AverageAmount) + (r*NewAmount)
        print(leftThresh,frontThresh,rightThresh,LDistance)
        
        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:
            # 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")
            return( (leftThresh,frontThresh,rightThresh) )

        if tr != "?":  # If TV remote button pressed
            import calib as Ca
            return( (Ca.leftThresh,Ca.frontThresh,Ca.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")

# Await hand wa
if screen:
    dsp.fill(0)
    dsp.text("Wave hand",0,0)
    dsp.show()
while True:
    (l,f,r) = readWalls()
    if f < FT*.7:
        break
led_onboard.on()
while True:
    (l,f,r) = readWalls()
    if f < 2000:
        break
led_onboard.off()

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


##############################
#####                     ####
#####     corridor()      ####
#####                     ####
##############################
def corridor():
    global tr
    print("corridor")
    BT("\nCor ")  # Bluetooth
    while True:
        if (tr == "1") or (tr == "2") or (tr == "3"):
            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:
            
            stop()
            if screen:
                dsp.fill(0)
                dsp.text("Front "+ str(f),0,0)
                dsp.show()
            BT("Cx ")  # Bluetooth    
            return
        if l < leftThresh/2:
            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 = speed+sDiff
        #lSpeed = min(lSpeed,65000)
        rSpeed = speed-sDiff
        #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/2 :
            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    

# new MRF-------



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 == "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 == "*":
            BOH()
        tr = "?"
        
            
        
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("button for next run",0,0)
        dsp.show()

    while button.value()==0:
        pass
    lifted = False
    #st()
    # write logstring to log file

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