Move esp8266 files to designated folder

[hubacji1/autiminipoci.git] / esp8266 / go_and_stop.py

diff --git a/esp8266/go_and_stop.py b/esp8266/go_and_stop.py
new file mode 100644 (file)
index 0000000..57c100a
--- /dev/null
+++ b/esp8266/go_and_stop.py
@@ -0,0 +1,50 @@
+# -*- coding: utf-8 -*-
+"""Run the car and stop if distance to obstacle is < 40cm."""
+from machine import Pin, PWM
+from utime import sleep_us, ticks_us
+
+TRIG = Pin(5, Pin.OUT)
+ECHO = Pin(4, Pin.IN)
+SPEED = PWM(Pin(14), freq=1000)
+BACK = PWM(Pin(15), freq=1000)
+LEFT = PWM(Pin(1d), freq=1000)
+RIGHT = PWM(Pin(13), freq=1000)
+
+dist = 40
+
+def go():
+    """Read data from ultrasonic sensor and drive car if no obstacle."""
+    global dist
+    BACK.duty(0)
+    LEFT.duty(0)
+    RIGHT.duty(0)
+    nc = ticks_us()
+    while True:
+        # read sensor data
+        TRIG.off()
+        sleep_us(2)
+        TRIG.on()
+        sleep_us(10)
+        TRIG.off()
+        while ECHO.value() == 0:
+            pass
+        t1 = ticks_us()
+        while ECHO.value() == 1:
+            pass
+        t2 = ticks_us()
+        dist = (t2 - t1) / 58.0
+        # drive if obstacle-free
+        if dist > 30:
+            LEFT.duty(0)
+            BACK.duty(0)
+            SPEED.duty(1023)
+        else:
+            SPEED.duty(0)
+            BACK.duty(1023)
+            LEFT.duty(1023)
+        nc += 100000
+        sleep_us(nc - ticks_us())
+    return
+
+if __name__ == "__main__":
+    go()