from utime import sleep
from machine import Pin
from hcsr04 import HCSR04
from stepper import Stepper

# Ultrasonic Sensor setup
sensor = HCSR04(trigger_pin=25, echo_pin=26)

# Stepper motor parameters
tire_diameter = 50  # in mm
steps_per_rev = 200
num_of_cycles = 5

# Function to measure distance
def measure_distance():
    distance = sensor.distance_cm()
    return distance

# Stepper motor setup
stepper = Stepper(dir_pin=16, step_pin=17, steps_per_rev=steps_per_rev, tire_diameter=tire_diameter, library='interrupt')

# Convert tire diameter to cm and calculate cm per revolution
tire_diameter_cm = tire_diameter / 10  # convert mm to cm
cm_per_revolution = 2 * 3.1415 * (tire_diameter_cm / 2)  # circumference in cm

# Main loop to check distance and move stepper
while True:
    distance = measure_distance()
    # If distance is less than 50 cm, move the stepper
    if distance < 50:
        
        stepper.move_x_centimeters(cm_per_revolution * num_of_cycles)
    else:
        break
    sleep(1)  # Wait for 1 second before the next measurement