#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
from ssd1306 import SSD1306_I2C
from machine import I2C

# set up dsp as device
i2c=I2C(1,sda=Pin(18),scl=Pin(19),freq=200000)
dsp=SSD1306_I2C(128,32,i2c)

dsp.fill_rect(0, 0, 127, 8, 0) # blank target row
dsp.text("Hello",0,0)  #print on target row   
dsp.show()                  # actually display
Hlen = 2
Vlen = 20
#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
#
FO_Adjust = 0.25
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 = 97.5
RockingDiamonds = 250
#three settings
swval = readswitches()
if swval == 0:
    normal = 80#45
    proportion = 0.6
    derivative = 160
elif swval == 1:
    normal = 88 #55
    proportion = 0.5 
    derivative = 190
elif swval == 2:
    normal = 88 
    proportion = 0.4 
    derivative = 210
elif swval == 3:
    normal = 90
    proportion = 0.3
    derivative = 210
print("deri ",derivative, "p ",proportion)

def showit(err):
    
    dsp.fill_rect(0, 0, 127, 48, 0) # blank target row
    dsp.fill_rect(62,25,4, 8,1)
    dsp.fill_rect(62+int(err),   8,  Hlen, Vlen,1)
    dsp.show()                  # actually display

'''
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'''
lcalib = 0
rcalib = 0

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:
        lcalib = int(100 * (lmax - lsensor_light) / (lmax - lmin))
    if rmax != rmin:
        rcalib = int(100 * (rmax - rsensor_light) / (rmax - rmin))
    if smax != smin:
        ssensor_light = int(100 * (smax - ssensor_light) / (smax - smin))
    print(ssensor_light)
    utime.sleep(0.1)#end of calibration
    if lcalib > FOlevel and rcalib > FOlevel:
        led_onboard.value(0)
        error = lcalib - rcalib#
    else:   #when it has fallen off
        led_onboard.value(1)
        if OldError > 0:
            error = FOError-(lcalib+rcalib)*FO_Adjust
        else:
            error = -FOError+(lcalib+rcalib)*FO_Adjust
    OldError = error
    showit(error/1.5)
    
led_onboard.value(0)
print("end of calib")

while True:
    ssensor_light = ssensor.read_u16()
    ssensor_light = int(100 * (smax - ssensor_light) / (smax - smin))
    if ssensor_light > 50:
        break
print("remove paper", ssensor_light)
led_onboard.value(1)
while True:
    ssensor_light = ssensor.read_u16()
    ssensor_light = int(100 * (smax - ssensor_light) / (smax - smin))
    if ssensor_light < 50:
        break
print("running")
led_onboard.value(0)
#turn on left red LED for 0.5 sec and wait 1 sec

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 < RockingDiamonds:
        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#
        if loopCount < 250:
            myspeed = BOH
        else:
            myspeed = normal
        
    else:   #when it has fallen off
        myspeed=normal-5
        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
##################
'''