from machine import Pin, I2C, PWM
import time
from hcsr04 import HCSR04
from i2c_lcd import I2cLcd

# Initialize I2C
i2c = I2C(scl=Pin(16), sda=Pin(21), freq=400000)

# Initialize LCD
lcd = I2cLcd(i2c, 0x27, 2, 16) 

# Initialize Ultrasonic Sensor
sensor = HCSR04(trigger_pin=32, echo_pin=35)

# Initialize Servo Motor
servo_pin = Pin(22)
servo = PWM(servo_pin, freq=50, duty=77)  # Adjust initial duty cycle

# Distance threshold for servo activation
DISTANCE_THRESHOLD = 100  # Adjust as needed

def measure_distance():
    distance = sensor.distance_cm()
    return distance

def display_distance(distance):
    lcd.clear()
    lcd.putstr("Distance:\n{} cm".format(distance))
    print('Distance:', distance, 'cm')

def main():
    while True:
        distance = measure_distance()
        display_distance(distance)
        
        # Control servo based on distance
        if distance < DISTANCE_THRESHOLD:
            servo.duty(77)  # Adjust duty cycle for the servo position
        else:
            servo.duty(30)  # Adjust duty cycle for the servo position

        print("Distance: {:.2f} cm".format(distance))
        time.sleep(5)  # Wait for 5 seconds before the next measurement

if __name__ == "__main__":
    main()