#This won the half size line follower competition at BCU on 18/03/2023
#Modifications needed:

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

#Digital inputs
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)       #Hw5 4th switch +++
button = Pin(22, Pin.IN)
#Digital outputs
led_onboard = Pin(25,Pin.OUT)
ledR = Pin(8, Pin.OUT)
ledL = Pin(13, Pin.OUT)
led = Pin(14, Pin.OUT)

left_dir = Pin(9, Pin.OUT)
right_dir = Pin(10, Pin.OUT)
#Analogue outputs
left_pwm = PWM(Pin(11))
right_pwm = PWM(Pin(12))
#Analogue inputs
lsensor = ADC(27)
rsensor = ADC(26)
ssensor = ADC(28)
#setup
left_pwm.freq(1000)
right_pwm.freq(1000)
left_pwm.duty_u16(0)   #reset
right_pwm.duty_u16(0)  #reset

left_dir.off() # off = forward
right_dir.off()


max_light = 0   # around 62000
min_light = 65000   # around 41000


lmax = 0
rmax = 0
smax = 0
lmin = 65000
rmin = 65000
smin = 65000
count = 0
flag = 0

lspeed = 0
rspeed = 0

OldError = 0
change = 0
speed_level = 655.35
#make changes here
normal = 55 #35
proportion = 0.55 #0.45
derivative = 20 #65
#
FOError = 60
FOlevel = 45  #larger makes it fall off sooner
FOSpeed = 30
def readswitches():
    swval = SW1.value() *1
    swval += SW2.value() *2
    swval += SW3.value() *4
    swval += SW4.value() *8
    return(swval)
BOH = 100
#three settings
swval = readswitches()
if swval == 0:
    normal = 80#45
    proportion = 0.6
    derivative = 145
elif swval == 1:
    normal = 55 
    proportion = 0.4 
    derivative = 70
elif swval == 2:
    normal = 65 
    proportion = 0.4 
    derivative = 95    
print("deri ",derivative, "p ",proportion)
'''
class PDController:
 def __init__(self, kp,kd):
     self.kp = kp
     self.kd = kd
     self.error_prev = 0
 def calculate(self, error, dt):
     difference = (error - self.error_prev)
     self.error_prev += difference
     self.derivative = difference / dt
     return self.kp * error + self.kd * self.derivative'''


led.value(1)#turn on sensor leds
while button.value() == 0:
    led_onboard.value(1)
    lsensor_light = lsensor.read_u16()
    rsensor_light = rsensor.read_u16()
    ssensor_light = ssensor.read_u16()
    lmax = max(lmax, lsensor_light)
    rmax = max(rmax, rsensor_light)
    lmin = min(lmin, lsensor_light)
    rmin = min(rmin, rsensor_light)
    smax = max(smax, ssensor_light)
    smin = min(smin, ssensor_light)
    print("lmax:", lmax, "lmin:", lmin, "rmax:", rmax, "rmin:s", rmin, "smax:", smax, "smin:", smin, "ssensor light:", ssensor_light, "count:", count)
    if lmax != lmin:
        lsensor_light = int(100 * (lmax - lsensor_light) / (lmax - lmin))
    if rmax != rmin:
        rsensor_light = int(100 * (rmax - rsensor_light) / (rmax - rmin))
    if smax != smin:
        ssensor_light = int(100 * (smax - ssensor_light) / (smax - smin))
        
    utime.sleep(0.1)
#turn on left red LED for 0.5 sec and wait 1 sec
ledL.value(1)
time.sleep(0.5)
ledL.value(0)
time.sleep(1)
loopCount = 0
while True and count < 2:
    lsensor_light = lsensor.read_u16()
    rsensor_light = rsensor.read_u16()
    ssensor_light = ssensor.read_u16()
    
    lsensor_light = int(100 * (lmax - lsensor_light) / (lmax - lmin))
    rsensor_light = int(100 * (rmax - rsensor_light) / (rmax - rmin))
    ssensor_light = int(100 * (smax - ssensor_light) / (smax - smin))
    print (ssensor_light)
    if loopCount < 500:
        myspeed = BOH
    else:
        myspeed = normal
    
    if ssensor_light < 50:
        ledR.value(1)
        flag = 1

    if ssensor_light > 50 and flag == 1:
        ledR.value(0)
        count = count + 1
        flag = 0
    
    """position = lsensor_light - rsensor_light
    print("left:", lsensor_light, "right:", rsensor_light)
    print("position:", position)
    utime.sleep(0.01)"""
    
    if lsensor_light > FOlevel and rsensor_light > FOlevel:
        led_onboard.value(0)
        error = lsensor_light - rsensor_light
        myspeed = normal
        
    else:   #when it has fallen off
        myspeed=normal-15
        led_onboard.value(1)
        if OldError > 0:
            error = FOError
        else:
            error = -FOError
    value = error * proportion
    change = error - OldError
    value = value + (change * derivative) / 10
    left_speed = myspeed - value
    right_speed = myspeed + value
    
    if left_speed >= 0:
        left_dir.off()
    else:
        left_dir.on()
        left_speed *= -1
    
    if right_speed >= 0:
        right_dir.off()
    else:
        right_dir.on()
        right_speed *= -1
    
    left_pwm.duty_u16(int(min((left_speed * speed_level), 65535)))
    right_pwm.duty_u16(int(min((right_speed * speed_level), 65535)))

    """print("position:", position,"left speed:", left_speed, "right speed:", right_speed)
    utime.sleep(0.5)
    print("left speed:", left_speed, "right speed:", right_speed)"""
    #print(left_speed)
    
    OldError = error
    time.sleep(0.005)
    loopCount += 1

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

'''
normal = 45 #35
proportion = 0.50 #0.45
derivative = 30 #65
##################
normal = 55 #35
proportion = 0.55 #0.45
derivative = 30 #65
=9.5sec
##################
'''