Implement backward steer to avoid obstacles

[hubacji1/autiminipoci.git] / go_and_stop.py

diff --git a/go_and_stop.py b/go_and_stop.py
index 454c8b3c1549d1f831e9d7a91011b06d2cce783f..57c100a00a3d2494026dde03772fd7044900e178 100644 (file)
--- a/go_and_stop.py
+++ b/go_and_stop.py
@@ -7,13 +7,17 @@ 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 get_dist():
+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
@@ -30,13 +34,17 @@ def get_dist():
         t2 = ticks_us()
         dist = (t2 - t1) / 58.0
         # drive if obstacle-free
-        if dist > 10:
+        if dist > 30:
+            LEFT.duty(0)
+            BACK.duty(0)
             SPEED.duty(1023)
         else:
             SPEED.duty(0)
-        nc += 20000
+            BACK.duty(1023)
+            LEFT.duty(1023)
+        nc += 100000
         sleep_us(nc - ticks_us())
     return
 
 if __name__ == "__main__":
-    get_dist()
+    go()