from hcsr04 import HCSR04
from time import sleep
from i2c_lcd import I2cLcd
from machine import Pin, ADC , SoftI2C
from stepper import STEPPER_MOTOR
stepper_L = STEPPER_MOTOR(step_pin=14, dir_pin=12)
stepper_R = STEPPER_MOTOR(step_pin=5, dir_pin=4)
stepper_H = STEPPER_MOTOR(step_pin=19, dir_pin=18)
i2c = SoftI2C(scl=Pin(22), sda=Pin(21), freq=400000)
lcd = I2cLcd(i2c, 0x27, 2, 16)
def RotateLeftServo() :
sleep(1)
stepper_L.rotate(0, 'forward') , stepper_R.rotate(360, 'forward')
lcd.move_to(0,1), lcd.putstr("Rotate Left")
def DefaulServo() :
sleep(1)
stepper_L.rotate(360, 'forward') , stepper_R.rotate(360, 'forward')
lcd.move_to(0,1), lcd.putstr("")
def RotateRightServo() :
sleep(1)
stepper_L.rotate(360, 'forward') , stepper_R.rotate(0, 'forward')
lcd.move_to(0,1), lcd.putstr("Rotate Right")
# ESP32
sensor = HCSR04(trigger_pin=25, echo_pin=26, echo_timeout_us=100000)
lcd.move_to(0,0)#cột hàng
lcd.putstr("Do Khoang Cach")
while True:
stepper_H.rotate(360, 'forward')
distance = sensor.distance_cm()
lcd.move_to(0,0), lcd.putstr("KC : " +str(distance) + "cm")
if distance <= 20 :
RotateRightServo()
else :
DefaulServo()
lcd.move_to(0,1), lcd.putstr(" ")
sleep(10)
RotateRightServo()
sleep(5)