main.py

from robot import Robot
from ultrasonic import Ultrasonic
us = Ultrasonic(13, 15)
rbt = Robot(us)
rbt.auto_pilot()

robot.py

import machine
import time


class Robot:

    FORWARD = 1
    REVERSE = 0

    MS_RUN = 100
    MS_WAIT = 500

    SPEED_LEFT = 1023
    SPEED_RIGHT = 1023
    SPEED_STOP = 0

    def __init__(self, us):
        """Constructor."""
        # Setup our ultrasonic sensor pins.
        self.us = us

        # Setup our motor pins
        self.left = machine.Pin(0, machine.Pin.OUT)
        self.right = machine.Pin(2, machine.Pin.OUT)
        self.left_speed = machine.PWM(machine.Pin(4), freq=1000, duty=self.SPEED_STOP)
        self.right_speed = machine.PWM(machine.Pin(5), freq=1000, duty=self.SPEED_STOP)

        # Just wait a second before we start.
        time.sleep(1)

    def auto_pilot(self):
        """Let the robot navigate by itself."""
        # Full speed
        self.left_speed.duty(self.SPEED_LEFT)
        self.right_speed.duty(self.SPEED_RIGHT)
  
        # Forward
        self.left.value(self.FORWARD)
        self.right.value(self.FORWARD)
  
        counter = 0
  
        while True:
            # Move forward
            self.left.value(self.FORWARD)
            self.right.value(self.FORWARD)
            self.left_speed.duty(self.SPEED_LEFT)
            self.right_speed.duty(self.SPEED_RIGHT)
            time.sleep_ms(self.MS_RUN)

            # Check our distance.
            distance = 0
            try:
                distance = self.us.distance_in_cm()
                print("Distance: {}".format(distance))
            except:
                print("Out of range")

            if distance <= 20 or distance > 500:
                print("Turning")
                self.right_speed.duty(self.SPEED_STOP)
                time.sleep_ms(self.MS_WAIT)
                self.right_speed.duty(self.SPEED_RIGHT)
                counter += 1
            else:
                counter = 0
  
            if counter >= 3:
                print("I'm stuck!")

                # Stop for half a second.
                self.left_speed.duty(self.SPEED_STOP)
                self.right_speed.duty(self.SPEED_STOP)
                time.sleep_ms(self.MS_WAIT)

                # Back it up.
                self.left.value(self.REVERSE)
                self.right.value(self.REVERSE)
                self.left_speed.duty(self.SPEED_LEFT)
                self.right_speed.duty(self.SPEED_RIGHT)
                time.sleep_ms(self.MS_WAIT)

                # Turn a bit.
                self.left.value(self.FORWARD)
                time.sleep_ms(self.MS_WAIT)

                # Try again.
                counter = 0
  
    def stop(self):
        """Stop the robot moving."""
        self.left_speed.duty(self.SPEED_STOP)
        self.right_speed.duty(self.SPEED_STOP)

ultrasonic.py

##
# Ultrasonic library for MicroPython's machineoard.
# Compatible with HC-SR04 and SRF04.
#
# Copyright 2014 - Sergio Conde Gómez <[email protected]>
# Improved by Mithru Vigneshwara
# Further enhancements by Dave Hall <[email protected]>
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program.  If not, see <http://www.gnu.org/licenses/>.
##

import machine
import time

class OutOfRangeError(Exception):
    pass

class Ultrasonic:

    """"Maximum measurable range (4 metres)."""
    MAX_RANGE = 23200

    def __init__(self, tPin, ePin):
        # WARNING: Don't use PA4-X5 or PA5-X6 as echo pin without a 1k resistor
        self.triggerPin = tPin
        self.echoPin = ePin

        # Init trigger pin (out)
        self.trigger = machine.Pin(self.triggerPin, machine.Pin.OUT, value=0)

        # Init echo pin (in)
        self.echo = machine.Pin(self.echoPin, machine.Pin.IN)

    def distance_in_inches(self):
        return (self.distance_in_cm() * 0.3937)

    def distance_in_cm(self):
        start = 0
        end = 0

        # Send a 10us pulse.
        self.trigger.high()
        time.sleep_us(10)
        self.trigger.low()

        # Wait 'till whe pulse starts.
        while self.echo.value() == 0:
            start = time.ticks_us()

        # Wait 'till the pulse is gone or 1000 iterations have passed.
        loops = 0
        while self.echo.value() == 1 and loops < 500:
            end = time.ticks_us()
            loops += 1

        duration = time.ticks_diff(end, start)
        if duration > self.MAX_RANGE or duration < 0:
            raise OutOfRangeError(str(duration))

        # Calc the duration of the recieved pulse, divide the result by
        # 2 (round-trip) and divide it by 29 (the speed of sound is
        # 340 m/s and that is 29 us/cm).
        dist_in_cm = (duration / 2) / 29

        return dist_in_cm