from Phidget22.Devices.DistanceSensor import *
import time

# Sonar Object
sonar = DistanceSensor()
# time-of-flight sensor
ds = DistanceSensor()

# Assign Ports
sonar.setHubPort(0)
ds.setHubPort(1)

# Open
sonar.openWaitForAttachment(1000)
ds.openWaitForAttachment(1000)

while True:
    try:
        # Get distance from sonar
        print(f"Distance {sonar.getDistance()}mm")
    except PhidgetException:
        try:
            # if too close get distance from time-of-flight
            print(f"Distance {ds.getDistance()}mm")
        except PhidgetException as err:
            print("Unable to measure distance.")
        
    time.sleep(0.1)

"""
# Old Code, used in Step 2 and 3

# Init new DistanceSensor Obj
ds = DistanceSensor()

# Open Distance Sensor, it will self attach
ds.openWaitForAttachment(1000)

# Start Loop
while True:
    # Get and print distance
    try:
        print(f"Distance: {ds.getDistance()}mm")
    except PhidgetException as err:
        print("Object outside of allowable range!")
    time.sleep(.1)
"""