from machine import Pin, PWM, time_pulse_us
import time

#Pin declaration for Ultrasonic
Trig = Pin(11, Pin.OUT)
Echo = Pin(10, Pin.IN)

#Pin declaration for Servo
Servo_Motor = PWM(Pin(40))
Servo_Motor.freq(50)

# Constants for controller
D_REF = 20    # desired distance in cm
DT = 1         # loop period in 1 second
Ik_0 = 0   #Ik(0) = 0
t = 0          #time delay
I = 0          #state variable
Kp, Ki, Kd = 2.0, 0.05, 0.1

#Controller gain
U_ON = 120        # servo angle for “move”
U_OFF = 60        # servo angle for “stop”

#function to read data from ultrasonic sensor
def measure_distance():
    #send 10 us pulse to trigger the sensor
    Trig.off()
    time.sleep_us(2)
    Trig.on()
    time.sleep_us(10)
    Trig.off()

    #measure the pulse
    pulse_duration = time_pulse_us(Echo, 1, 30000)

    #calculate the distance
    distance_cm = (pulse_duration/2)/29.1 
    return distance_cm

#function to move the servo motor
def servo_angle (angle):
    min_duty = 25 #set PWM min value = 0 degree
    max_duty = 125 #set PWM max value = 180 degree
    duty = min_duty +((angle/180)*(max_duty-min_duty))
    Servo_Motor.duty(int(duty))

#start the controller session
#I_Prev = 0
I = 0

while True:
    #Read the distance from ultrasonic sensor
    distance = measure_distance()
    time.sleep(0.5)
    e = distance - D_REF
    t += 1

    I_Prev = I

    #PI Controller, Let say start from 90 degree
    I = I_Prev + (e * DT)
    u = 90 + (Kp*e) + (Ki*I)
    u = max(0, min(180, u)) # clamp u to [0,180]
    

    #send angle to servo motor    
    servo_angle(u)

    print(f"I_Prev = {I_Prev}, I = {I}")
    print(f"t= {t},Robot Distance = {distance:.1f}, Error = {e:.1f}, Servo Angle = {u:.1f}")
    time.sleep(0.5)