From ffbf200bda5cfb84ea0a534b28215182ba78f66d Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Thu, 21 Feb 2019 12:15:34 +0100
Subject: [PATCH] Add basic stop-if-near module

---
 go-and-stop.py | 50 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 50 insertions(+)
 create mode 100644 go-and-stop.py

diff --git a/go-and-stop.py b/go-and-stop.py
new file mode 100644
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
-- 
2.39.2