#!/usr/bin/python
# ========================================================
# Python script for PiBot-A: obstacle avoidance
# Version 0.9 - by Thomas Schoch - www.retas.de
# ========================================================
from pololu_drv8835_rpi import motors, MAX_SPEED
from time import sleep
import wiringpi2 as wp2
# Signal handler for SIGTERM
import signal, sys
def sigterm_handler(signal, frame):
motors.setSpeeds(0, 0)
sys.exit(0)
signal.signal(signal.SIGTERM, sigterm_handler)
# GPIO pins of sensors
GPIO_right = 21
GPIO_middle = 26
GPIO_left = 20
# Configure sensors as input
wp2.pinMode(GPIO_right, wp2.GPIO.INPUT)
wp2.pinMode(GPIO_middle, wp2.GPIO.INPUT)
wp2.pinMode(GPIO_left, wp2.GPIO.INPUT)
try:
# Start moving forward
motors.setSpeeds(MAX_SPEED, MAX_SPEED)
while True: # Main loop
# Read sensor input (positive logic)
INPUT_right = not wp2.digitalRead(GPIO_right)
INPUT_middle = not wp2.digitalRead(GPIO_middle)
INPUT_left = not wp2.digitalRead(GPIO_left)
# Set motor speeds dependent on sensor input
if INPUT_left and INPUT_right:
# Obstacle immediately ahead: move a bit bwrd,
# turn left a little bit and then proceed fwrd
motors.setSpeeds(-200, -200)
sleep (1)
motors.setSpeeds(-200, 200)
sleep (0.3)
motors.setSpeeds(MAX_SPEED, MAX_SPEED)
elif INPUT_middle: # turn left
motors.setSpeeds(100, MAX_SPEED)
elif INPUT_left: # turn right
motors.setSpeeds(MAX_SPEED, 200)
elif INPUT_right: # turn left
motors.setSpeeds(200, MAX_SPEED)
else:
# No sensor input: drive forward
motors.setSpeeds(MAX_SPEED, MAX_SPEED)
# Repeat this loop every 0.1 seconds
sleep (0.1)
finally:
# Stop motors in case of <Ctrl-C> or SIGTERM:
motors.setSpeeds(0, 0)