go_and_stop.py

   1 # -*- coding: utf-8 -*-
   2 """Run the car and stop if distance to obstacle is < 40cm."""
   3 from machine import Pin, PWM
   4 from utime import sleep_us, ticks_us
   5
   6 TRIG = Pin(5, Pin.OUT)
   7 ECHO = Pin(4, Pin.IN)
   8 SPEED = PWM(Pin(14), freq=1000)
   9 BACK = PWM(Pin(15), freq=1000)
  10 LEFT = PWM(Pin(1d), freq=1000)
  11 RIGHT = PWM(Pin(13), freq=1000)
  12
  13 dist = 40
  14
  15 def go():
  16     """Read data from ultrasonic sensor and drive car if no obstacle."""
  17     global dist
  18     BACK.duty(0)
  19     LEFT.duty(0)
  20     RIGHT.duty(0)
  21     nc = ticks_us()
  22     while True:
  23         # read sensor data
  24         TRIG.off()
  25         sleep_us(2)
  26         TRIG.on()
  27         sleep_us(10)
  28         TRIG.off()
  29         while ECHO.value() == 0:
  30             pass
  31         t1 = ticks_us()
  32         while ECHO.value() == 1:
  33             pass
  34         t2 = ticks_us()
  35         dist = (t2 - t1) / 58.0
  36         # drive if obstacle-free
  37         if dist > 30:
  38             LEFT.duty(0)
  39             BACK.duty(0)
  40             SPEED.duty(1023)
  41         else:
  42             SPEED.duty(0)
  43             BACK.duty(1023)
  44             LEFT.duty(1023)
  45         nc += 100000
  46         sleep_us(nc - ticks_us())
  47     return
  48
  49 if __name__ == "__main__":
  50     go()