from machine import Pin, I2C
from time import sleep
from buzzer import Buzzer
import oled
import mpu6050
from stepper import Stepper
from hcsr04 import HCSR04
i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=400000)
led = Pin(12, Pin.OUT)
button = Pin(27, Pin.IN, Pin.PULL_UP)
buzzer = Buzzer(15)
mpu = mpu6050.accel(i2c)
oled = oled.SSD1306_I2C(128, 64, i2c)
trigger_pin = 5
echo_pin = 18
ultrasonic = HCSR04(trigger_pin, echo_pin)
left_step = Stepper([13])
right_step = Stepper([19])
def wait_button_press():
while button.value() == 1:
pass
def get_steps_from_distance(distance_cm):
steps_per_cm = 200 / (2 * 3.1416 * 3)
steps = int(distance_cm * steps_per_cm)
return steps
while True:
led.value(0)
oled.fill(0)
oled.text("Press the button", 1, 20)
oled.text("to start Robot", 15, 35)
oled.show()
wait_button_press()
led.value(1)
buzzer.beep_once()
distance_cm = ultrasonic.get_distance_cm()
steps = get_steps_from_distance(distance_cm)
oled.fill(0)
oled.text("{:.2f}cm".format(distance_cm), 10, 20)
oled.text("{} steps".format(steps), 10, 40)
oled.show()
sleep(1)
reached = True
for _ in range(steps):
left_step.move_one_step()
right_step.move_one_step()
sleep(0.005)
values = mpu.get_values()
y_axis = values["AcY"]
if y_axis > 10000 or y_axis < -10000:
reached = False
break
if reached:
oled.fill(0)
oled.text("Destination Reached", 30, 30)
oled.show()
buzzer.beep_once()
else:
led.value(1)
oled.fill(0)
oled.text("Flipped Over", 30, 30)
oled.show()
for _ in range(3):
buzzer.beep_once()
sleep(0.5)
sleep(2.5)
Right Motor
Left Motor