225 lines
5.9 KiB
Python
225 lines
5.9 KiB
Python
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) |