# Project objective: Swing the servo arm from min, mid, to max positions
#
# Hardware and connections used:
#   Servo GND to Raspberry Pi Pico GND
#   Servo V+ to Raspberry Pi Pico 3.3 V
#   Servo PWM pin to GPIO Pin 15
#   Servo PWM pin to GPIO Pin 13
# 
# Programmer: Adrian Josele G. Quional

# modules
from picozero import Servo
from time import sleep
from machine import Pin, time_pulse_us
from utime import sleep_us, sleep_ms

# creating a Servo object
servo = Servo(15)
servo1 = Servo(13)

TRIG_PULSE_DURATION_US = 10
ULTRASONIC_MEASURE_TIMEOUT = 38000

#define ultrasonic pins, trig, echo
trig_pin1 = Pin(16, Pin.OUT)
echo_pin1 = Pin(17, Pin.IN)
trig_pin2 = Pin(19, Pin.OUT)
echo_pin2 = Pin(20, Pin.IN)

SOUND_SPEED = 343000  # Sound speed in mm/s

def calc_ultrasonic_distance(time_in_us, speed_of_sound):
    # divide by 1000000 to get the time in seconds and divide by 2 since
    # the time measured is two way
    ultrasonic_duration = (time_in_us / 1000000) / 2
    return speed_of_sound * ultrasonic_duration

# continuously swing the servo arm to min, mid, and max positions (for a duration of 1 sec each)
while True:
    # swinging the servo arm to its min and max position
    servo.min()
    sleep(1)
    servo1.max()
    sleep(1)

    # swinging the servo arm to its mid position
    servo.mid()
    sleep(1)
    servo1.mid()
    sleep(1)

    # swinging the servo arm to its min and max position
    servo.max()
    sleep(1)
    servo1.min()
    sleep(1)

    # Prepare to signal
    trig_pin1.off()
    trig_pin2.off()
    sleep_us(5)

    # Send pulse of 10 µs
    trig_pin1.on()
    sleep_us(TRIG_PULSE_DURATION_US)
    trig_pin1.off()
 
    # use time_pulse_us, to measure the echo pin time duration
    # time returned is in microseconds
    ultrasonic_duration1 = time_pulse_us(echo_pin1, 1, ULTRASONIC_MEASURE_TIMEOUT)
    distance_mm1 = calc_ultrasonic_distance(ultrasonic_duration1, SOUND_SPEED)

    # Send pulse of 10 µs
    trig_pin2.on()
    sleep_us(TRIG_PULSE_DURATION_US)
    trig_pin2.off()

    ultrasonic_duration2 = time_pulse_us(echo_pin2, 1, ULTRASONIC_MEASURE_TIMEOUT)
    distance_mm2 = calc_ultrasonic_distance(ultrasonic_duration2, SOUND_SPEED)

    print(f"Distance sensor 1: {distance_mm1} mm")
    print(f"Distance sensor 2: {distance_mm2} mm")

    # Adjust sleep_ms() according to your application requirements
    sleep_ms(100)