Implement backward steer to avoid obstacles

diff --git a/go_and_stop.py b/go_and_stop.py
index 5941b2bd0bf19f32ffc167c1647ee2c3dd1c02ec..57c100a00a3d2494026dde03772fd7044900e178 100644 (file)
--- a/go_and_stop.py
+++ b/go_and_stop.py
@@ -7,6 +7,8 @@ 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
 
@@ -14,6 +16,8 @@ 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,11 +34,15 @@ def go():
         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