Programming Autonomous Raspberry Pi Robot with Python – Part3

We are in part 4 of tutorial series of Building Autonomous Raspberry Pi Controlled Robot! If you want to jump back and forth or missed any of my previous tutorials – here is the consolidated list.

All Tutorials to build autonomous Pi Robot:

  1. Build Autonomous Raspberry Pi Robot
  2. Adding Ultrasonic Sensor to Raspberry Pi Robot
  3. Adding Servo to Raspberry Pi Robot
  4. Programming Autonomous Raspberry Pi Robot with Python
  5. Final Autonomous Raspberry Pi robot in Action

If you want to see all my Raspberry Pi Robotics tutorials do visit this page – http://www.vishalavalani.com/robotics-tutorials/

Its programming time and giving brain to the Robot! Hope you have assembled all the components i.e. servo motor and ultrasonic sensor to Raspberry Pi. Here is the consolidated connection diagram if you have missed the tutorial:

Declaring Needed Variables and Initialization

Below code is self-explanatory:

import RPi.GPIO as GPIO
import time

GPIO.setmode(GPIO.BOARD)
GPIO.setup(11, GPIO.OUT)
GPIO.setup(13, GPIO.OUT)
GPIO.setup(15, GPIO.OUT)
GPIO.setup(16, GPIO.OUT)

TRIG = 18
ECHO = 22
servo_pin = 40
duty_cycle = 7.5
distance_obstacle = 40

GPIO.setup(TRIG, GPIO.OUT)
GPIO.setup(ECHO, GPIO.IN)
GPIO.setup(servo_pin, GPIO.OUT)

GPIO.output(TRIG, False)
pwm_servo = GPIO.PWM(servo_pin, 50)
pwm_servo.start(duty_cycle)

Functions

Best practice is to convert repetitive code into function. Here, we have following below functions with their own onus:

  • ReadSensorReading – Read from the sensor
  • Forward – Go forward
  • Reverse – Go reverse
  • Right – Turn right
  • Left – Turn left
  • Brake – Stop the robot
  • GetSensorReadings – Get the readings to send it to main function
def ReadSensorReading():
    GPIO.output(TRIG, False)  # Set TRIG as LOW
    time.sleep(0.1)  # Delay

    GPIO.output(TRIG, True)
    time.sleep(0.00001)
    GPIO.output(TRIG, False)

    while GPIO.input(ECHO) == 0:
        pulse_start = time.time()

    while GPIO.input(ECHO) == 1:
        pulse_end = time.time()

    pulse_duration = pulse_end - pulse_start
    distance = pulse_duration * 17150
    distance = round(distance, 2)
    return distance


def Forward():
    GPIO.output(11, 1)
    GPIO.output(13, 0)
    GPIO.output(15, 1)
    GPIO.output(16, 0)


def Reverse():
    GPIO.output(11, 0)
    GPIO.output(13, 1)
    GPIO.output(15, 0)
    GPIO.output(16, 1)


def Right():
    GPIO.output(11, 1)
    GPIO.output(13, 0)
    GPIO.output(15, 0)
    GPIO.output(16, 1)


def Left():
    GPIO.output(11, 0)
    GPIO.output(13, 1)
    GPIO.output(15, 1)
    GPIO.output(16, 0)


def Brake():
    GPIO.output(11, 1)
    GPIO.output(13, 1)
    GPIO.output(15, 1)
    GPIO.output(16, 1)


def GetSensorReadings():
    pwm_servo.ChangeDutyCycle(13)
    time.sleep(0.1)
    distincmsleft = ReadSensorReading()
    print "distincmsleft " + str(distincmsleft)

    time.sleep(0.1)
    pwm_servo.ChangeDutyCycle(10)
    time.sleep(0.1)
    distincmsleft1 = ReadSensorReading()
    print "distincmsleft1 " + str(distincmsleft1)

    time.sleep(0.1)
    pwm_servo.ChangeDutyCycle(7.5)
    time.sleep(0.1)
    distincmsstraight = ReadSensorReading()
    print "distincmsstraight " + str(distincmsstraight)

    time.sleep(0.1)
    pwm_servo.ChangeDutyCycle(5)
    time.sleep(0.1)
    distincmsright1 = ReadSensorReading()
    print "distincmsright1 " + str(distincmsright1)

    time.sleep(0.1)
    pwm_servo.ChangeDutyCycle(3)
    time.sleep(0.1)
    distincmsright = ReadSensorReading()
    print "distincmsright " + str(distincmsright)
    time.sleep(0.1)
    return distincmsleft, distincmsleft1, distincmsstraight, distincmsright1, distincmsright

Master Logic

First we will create infinite loop and read the sensor readings:

distincmsleft, distincmsleft1, distincmsstraight, distincmsright1, distincmsright = GetSensorReadings()

Now we will detect if there is any obstacle in front first:

Master case 1:

if distincmsstraight < distance_obstacle:

If there is any obstacle in front, we should stop the robot and then look for left and right readings. If the there is obstacle on left – turn right and vice versa. If there is no obstacle on either side, we are prioritising right turn here which is handled in else case. Below code does the trick:

if distincmsstraight &lt; distance_obstacle:
    Brake()
    if distincmsleft &lt; distance_obstacle or distincmsleft1 &lt; distance_obstacle:
        Brake()
        for x in range(1, 3, 1):
            Right()
            time.sleep(0.1)
            Brake()
    elif distincmsright &lt; distance_obstacle or distincmsright1 &lt; distance_obstacle:
        Brake()
        for x in range(1, 3, 1):
            Left()
            time.sleep(0.1)
            Brake()
    else:
        Brake()
        for x in range(1, 3, 1):
            Right()
            time.sleep(0.1)
            Brake()

Master case 2:

elif distincmsleft &lt; distance_obstacle or distincmsleft1 &lt; distance_obstacle:

Check if there is obstacle on left – if there is any obstacle turn right which is handled by below code:

Brake()
for x in range(1, 3, 1):
    Right()
    time.sleep(0.1)
    Brake()

Master case 3:

Check if there is obstacle on right – if there is any obstacle turn left which is handled by below code:

elif distincmsright &lt; distance_obstacle or distincmsright1 &lt; distance_obstacle:
    Brake()
    for x in range(1, 3, 1):
        Left()
        time.sleep(0.1)
        Brake()

Master case 4:

If none of above master cases are observed – simply move forward for 0.5 second and then again perform the same operation again! 🙂

else:
    Forward()
    time.sleep(0.5)
    Brake()

Putting it all together

import RPi.GPIO as GPIO
import time

GPIO.setmode(GPIO.BOARD)
GPIO.setup(11, GPIO.OUT)
GPIO.setup(13, GPIO.OUT)
GPIO.setup(15, GPIO.OUT)
GPIO.setup(16, GPIO.OUT)

TRIG = 18
ECHO = 22
servo_pin = 40
duty_cycle = 7.5
distance_obstacle = 40

GPIO.setup(TRIG, GPIO.OUT)
GPIO.setup(ECHO, GPIO.IN)
GPIO.setup(servo_pin, GPIO.OUT)

GPIO.output(TRIG, False)
pwm_servo = GPIO.PWM(servo_pin, 50)
pwm_servo.start(duty_cycle)
print "Waiting For Sensor To Settle"
time.sleep(2)


def ReadSensorReading():
    GPIO.output(TRIG, False)  # Set TRIG as LOWd
    time.sleep(0.1)  # Delay

    GPIO.output(TRIG, True)
    time.sleep(0.00001)
    GPIO.output(TRIG, False)

    while GPIO.input(ECHO) == 0:
        pulse_start = time.time()

    while GPIO.input(ECHO) == 1:
        pulse_end = time.time()

    pulse_duration = pulse_end - pulse_start
    distance = pulse_duration * 17150
    distance = round(distance, 2)
    return distance


def Forward():
    GPIO.output(11, 1)
    GPIO.output(13, 0)
    GPIO.output(15, 1)
    GPIO.output(16, 0)


def Reverse():
    GPIO.output(11, 0)
    GPIO.output(13, 1)
    GPIO.output(15, 0)
    GPIO.output(16, 1)


def Right():
    GPIO.output(11, 1)
    GPIO.output(13, 0)
    GPIO.output(15, 0)
    GPIO.output(16, 1)


def Left():
    GPIO.output(11, 0)
    GPIO.output(13, 1)
    GPIO.output(15, 1)
    GPIO.output(16, 0)


def Brake():
    GPIO.output(11, 1)
    GPIO.output(13, 1)
    GPIO.output(15, 1)
    GPIO.output(16, 1)


def GetSensorReadings():
    pwm_servo.ChangeDutyCycle(13)
    time.sleep(0.1)
    distincmsleft = ReadSensorReading()
    print "distincmsleft " + str(distincmsleft)

    time.sleep(0.1)
    pwm_servo.ChangeDutyCycle(10)
    time.sleep(0.1)
    distincmsleft1 = ReadSensorReading()
    print "distincmsleft1 " + str(distincmsleft1)

    time.sleep(0.1)
    pwm_servo.ChangeDutyCycle(7.5)
    time.sleep(0.1)
    distincmsstraight = ReadSensorReading()
    print "distincmsstraight " + str(distincmsstraight)

    time.sleep(0.1)
    pwm_servo.ChangeDutyCycle(5)
    time.sleep(0.1)
    distincmsright1 = ReadSensorReading()
    print "distincmsright1 " + str(distincmsright1)

    time.sleep(0.1)
    pwm_servo.ChangeDutyCycle(3)
    time.sleep(0.1)
    distincmsright = ReadSensorReading()
    print "distincmsright " + str(distincmsright)
    time.sleep(0.1)
    return distincmsleft, distincmsleft1, distincmsstraight, distincmsright1, distincmsright


try:
    while (True):
        distincmsleft, distincmsleft1, distincmsstraight, distincmsright1, distincmsright = GetSensorReadings()
        if distincmsstraight &lt; distance_obstacle:
            Brake()
            if distincmsleft &lt; distance_obstacle or distincmsleft1 &lt; distance_obstacle:
                Brake()
                for x in range(1, 3, 1):
                    Right()
                    time.sleep(0.1)
                    Brake()
            elif distincmsright &lt; distance_obstacle or distincmsright1 &lt; distance_obstacle:
                Brake()
                for x in range(1, 3, 1):
                    Left()
                    time.sleep(0.1)
                    Brake()
            else:
                Brake()
                for x in range(1, 3, 1):
                    Right()
                    time.sleep(0.1)
                    Brake()
        elif distincmsleft &lt; distance_obstacle or distincmsleft1 &lt; distance_obstacle:
            Brake()
            for x in range(1, 3, 1):
                Right()
                time.sleep(0.1)
                Brake()
        elif distincmsright &lt; distance_obstacle or distincmsright1 &lt; distance_obstacle:
            Brake()
            for x in range(1, 3, 1):
                Left()
                time.sleep(0.1)
                Brake()
        else:
            Forward()
            time.sleep(0.5)
            Brake()
finally:
    pwm_servo.ChangeDutyCycle(7.5)
    Brake()
    print("Cleaning Up!")
    GPIO.cleanup()
Please follow and like us:

Leave a Reply

Your email address will not be published. Required fields are marked *