First commit

main.py

@@ -0,0 +1,225 @@
+from machine import SoftI2C, Pin, time_pulse_us
+from utime import sleep
+
+from led import LED
+from button import Button
+from buzzer import Buzzer
+from stepper import Stepper
+from hcsr04 import HCSR04
+import oled
+import mpu6050
+
+led        = LED(12)        # D12
+button     = Button(27)     # D27
+buzzer     = Buzzer(15)     # D15
+leftWheel  = Stepper(13)    # Step: D13
+rightWheel = Stepper(19)    # Step: D19
+ultrasonic = HCSR04(5, 18)  # Trigger: D5, Echo: 18
+
+i2c = SoftI2C(scl=Pin(22), sda=Pin(21))
+oled = oled.I2C(128, 64, i2c)
+mpu = mpu6050.accel(i2c)
+
+# 1 cm -> 11 steps
+# 10 cm -> 106 steps
+# 49 cm -> 520 steps
+def get_steps_from_distance(distance):
+    wheel_diameter = 6  # centimeters
+    wheel_circumference = wheel_diameter * 3.14159265
+    steps_per_revolution = 200
+    return int(
+        distance * steps_per_revolution / wheel_circumference
+    )
+
+# ------------------------- Part 1: Test -------------------------
+
+# while True:
+#     value = button.pin.value()
+#     print(value)
+#     if button.is_pressed():
+#         led.on()
+#         buzzer.beep_once()
+#     else:
+#         led.off()
+#     sleep(0.5)
+
+# while True:
+#     led.off()
+#     # Program pauses here until the button is pressed
+#     button.wait_button_press()
+#     led.on()
+#     buzzer.beep_once()
+#     # Wait until the button is released before restarting
+#     while button.is_pressed():
+#         sleep(0.01)
+
+# ------------------------- Part 2: Test -------------------------
+
+# while True:
+#     if button.is_pressed():
+#         # Opposite directions make the two robot wheels move forward
+#         leftWheel.move_one_step()
+#         rightWheel.move_one_step()
+#     else:
+#         sleep(0.01)
+
+# ------------------------- Part 3: Test -------------------------
+
+# while True:
+#     try:
+#         distance = ultrasonic.distance_cm()
+#         print("Distance:", distance, "cm")
+#     except OSError as error:
+#         print("Ultrasonic error:", error)
+#     sleep(0.5)
+
+# test_distances = (10, 20, 30, 50, 100)
+# for distance in test_distances:
+#     steps = get_steps_from_distance(distance)
+#     print(distance, "cm =", steps, "steps")
+
+# while True:
+#     led.off()
+#     print("Press the button to measure and move")
+#     button.wait_button_press()
+#     try:
+#         distance = ultrasonic.distance_cm()
+#         target_steps = get_steps_from_distance(distance)
+#         print("Distance:", distance, "cm")
+#         print("Target steps:", target_steps)
+#         led.on()
+#         for step in range(target_steps):
+#             leftWheel.move_one_step()
+#             rightWheel.move_one_step()
+#         buzzer.beep_once()
+#         print("Movement complete")
+#     except OSError as error:
+#         print("Ultrasonic error:", error)
+#     # Prevent another measurement while the button remains held
+#     while button.is_pressed():
+#         sleep(0.01)
+
+# ------------------------- Part 4: Test -------------------------
+
+# print("I2C devices:", [hex(address) for address in i2c.scan()])
+# I2C devices: ['0x3c', '0x68']
+
+# OLED test
+# while True:
+#     try:
+#         distance = ultrasonic.distance_cm()
+#         print("Distance:", distance, "cm")
+#         oled.clear()
+#         oled.text("Distance:", 0, 1)
+#         oled.text(str(distance) + " cm", 0, 2)
+#         oled.show()
+#     except OSError as error:
+#         print("Ultrasonic error:", error)
+#         oled.clear()
+#         oled.text("Sensor error", 0, 2)
+#         oled.show()
+#     sleep(0.5)
+
+# MPU6050 test
+# while True:
+#     values = mpu.get_values()
+#     ac_y = values["AcY"]
+#     print("AcY:", ac_y)
+#     oled.clear()
+#     oled.text("Accelerometer", 0, 1)
+#     oled.text("AcY:", 0, 2)
+#     oled.text(str(ac_y), 0, 3)
+#     oled.show()
+#     sleep(0.25)
+
+# Combined test
+# while True:
+#     try:
+#         distance = ultrasonic.distance_cm()
+#         values = mpu.get_values()
+#         ac_y = values["AcY"]
+#         print("Distance:", distance, "cm")
+#         print("AcY:", ac_y)
+#         oled.clear()
+#         oled.text("Distance: " + str(distance), 0, 1)
+#         oled.text("cm", 0, 2)
+#         oled.text("AcY: " + str(ac_y), 0, 4)
+#         oled.show()
+#     except OSError as error:
+#         print("Ultrasonic error:", error)
+#     sleep(0.5)
+
+# ------------------------- Final Project -------------------------
+
+while True:
+
+    # Set LED to off, # IDLE state
+    led.off()
+
+    oled.clear()
+    oled.text("Press button", 0, 2)
+    oled.text("To Start", 0, 3)
+    oled.show()
+
+    # Wait here until the button is pressed
+    button.wait_button_press()
+    buzzer.beep_once()
+
+    # Read the ultrasonic distance
+    try:
+        distance = ultrasonic.distance_cm()
+    except OSError:
+        display.clear()
+        display.text("Sensor error", 0, 3)
+        display.show()
+        sleep(3)
+        # Require the button to be released
+        while button.is_pressed():
+            sleep(0.01)
+        continue
+
+    target_steps = get_steps_from_distance(distance)
+    
+    # Display the measured distance and steps
+    oled.clear()
+    oled.text("Distance: " + str(distance), 0, 2)
+    oled.text("Steps: " + str(target_steps), 0, 3)
+    oled.show()
+
+    reached = True
+
+    # Drive the required number of steps
+    for _ in range(target_steps):
+        leftWheel.move_one_step()
+        rightWheel.move_one_step()
+
+        # Check the accelerometer after each step
+        values = mpu.get_values()
+        ac_y = values["AcY"]
+
+        if ac_y > 12000 or ac_y < -12000:
+            reached = False
+            break
+
+    oled.clear()
+
+    if reached:
+        oled.text("REACHED", 0, 3)
+        oled.show()
+        buzzer.beep_once()
+    else:
+        led.on()
+        oled.text("TILTED", 0, 3)
+        oled.show()
+        # Beep three times
+        for beep in range(3):
+            buzzer.beep_once()
+            if beep < 2:
+                sleep(0.2)
+
+    # Keep the result visible
+    sleep(3)
+
+    # Require a new button press for the next run
+    while button.is_pressed():
+        sleep(0.01)