Add basic stop-if-near module

diff --git a/go-and-stop.py b/go-and-stop.py
new file mode 100644 (file)
index 0000000..8bb55d5
--- /dev/null
+++ b/go-and-stop.py
@@ -0,0 +1,50 @@
+# -*- coding: utf-8 -*-
+"""Run the car and stop if distance to obstacle is < 40cm."""
+from _thread import start_new_thread
+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(12), freq=1000)
+
+dist = 4
+
+def get_dist():
+    """Read data from ultrasonic sensor."""
+    global dist
+    nc = ticks_us()
+    while True:
+        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
+        nc += 20000
+        sleep_us(nc - ticks_us())
+    return
+
+def set_speed():
+    """Set PWM to control car speed."""
+    nc = ticks_us()
+    while True:
+        if dist < 20:
+            SPEED.duty(1023)
+        else:
+            SPEED.duty(0)
+        nc += 200000
+        sleep_us(nc - ticks_us())
+    return
+
+if __name__ == "__main__":
+    start_new_thread(get_dist, ())
+    start_new_thread(set_speed, ())
+    while True:
+        pass