#!/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)