from machine import Pin, PWM
import utime

# Pin Definitions
# Second ultrasonic sensor (for relay control)
trigger2 = Pin(14, Pin.OUT)  # Trigger pin for second sensor
echo2 = Pin(13, Pin.IN)      # Echo pin for second sensor

servo = PWM(Pin(5))          # Servo motor on GPIO 5
led = Pin(25, Pin.OUT)
relay = Pin(6, Pin.OUT)    
relay2 = Pin(10, Pin.OUT) 
servo.freq(50)               # Set PWM frequency to 50Hz (standard for servos)

# Function to set servo angle (0 to 180 degrees)
def set_servo_angle(angle):
    duty = int(40 + (angle / 180) * 115)  # Map angle to duty cycle (40 to 155)
    servo.duty_u16(duty * 65535 // 1023)  # Convert duty to 16-bit value
    utime.sleep_ms(500)  # Give the servo time to move

# Function to read distance from ultrasonic sensor
def read_distance(trigger_pin, echo_pin):
    # Send a 10us pulse to trigger the sensor
    trigger_pin.low()
    utime.sleep_us(2)
    trigger_pin.high()
    utime.sleep_us(5)
    trigger_pin.low()

    # Wait for the echo pin to go high
    while echo_pin.value() == 0:
        signaloff = utime.ticks_us()

    while echo_pin.value() == 1:
        signalon = utime.ticks_us()

    # Calculate distance in centimeters
    timepassed = signalon - signaloff
    distance = (timepassed * 0.0343) / 2  # Speed of sound is 343 m/s (0.0343 cm/us)
    return distance

def ultra():
    # Read sensor (for relay control)
    distance = read_distance(trigger2, echo2)
    print("The Distance from object is", distance, "cm")

    # Control relay based on sensor distance
    if distance < 20:
        relay.value(1) 
        relay2.value(1)  # Turn relay ON
        print("Relay activated (distance < 20cm)")
    else:
        relay.value(0)  # Turn relay OFF
        print("Relay deactivated")

while True:
    ultra()
    utime.sleep(1)