Start Here Robot

#!usr/bin/env python
import RPIO as GPIO
from GPIO import PWM
import time

#Motor Pin Declarations
AIA = 25
AIB = 24
BIA = 23
BIB = 18


#Servo Pin Declarations
"servoPin = 17"
"servoMinPulse = 800" #Minimum Pulse width the servo can safely take
"servoMaxPulse = 1900"    #Maximum pulse width the servo can safely take

"servoCentre = servoMinPulse + ((servoMaxPulse -" "servoMinPulse) / 2)"

#Ultrasound
trigger = 8
echo = 7
dangerLevel = 30    #Distance in centimetres at which to avoid an object
exitLevel = 10  #Distance in centimetres at which the program should exit

#GPIO Initialisations
GPIO.setup(AIA, GPIO.OUT, initial = inactiveState)
GPIO.setup(AIB, GPIO.OUT, initial = inactiveState)
GPIO.setup(BIA, GPIO.OUT, initial = inactiveState)
GPIO.setup(BIB, GPIO.OUT, initial = inactiveState)

"servo = PWM.Servo()"
"servo.output(servoPin, servoCentre)"

GPIO.setup(trigger, GPIO.OUT, initial = GPIO.LOW)
GPIO.setup(echo, GPIO.IN)

time.sleep(0.5) #Gives all I/O devices a chance to settle down
#mainly the ultrasound and servo

running = True
distance = dangerLevel * 2  #Just a semi random init value

while running == True:
    GPIO.output(AIA, 0)
    GPIO.output(AIB, 1)
    GPIO.output(BIA, 0)
    GPIO.output(BIB, 1)
    while distance > dangerLevel:
        GPIO.output(trigger, True)
        time.sleep(0.00001)
        GPIO.output(trigger, False)
        start = time.time()
        while GPIO.input(echo) == 0:
            start = time.time()
        while GPIO.input(echo) == 1:
            stop = time.time()
        elapsed = stop - start
        distance = elapsed * 34000
        distance = distance / 2
        print distance
    print "Danger Level Hit"
    #Stop
    GPIO.output(AIA, True)
    GPIO.output(AIB, True)
    GPIO.output(BIA, True)
    GPIO.output(BIB, True)
    if distance > exitLevel:
        print "Obstacle, Avoid"
        GPIO.output(AIA, 1)
        GPIO.output(AIB, 0)
        GPIO.output(BIA, 0)
        GPIO.output(BIB, 1)
        while distance <= dangerLevel:
            GPIO.output(trigger, True)
            time.sleep(0.00001)
            GPIO.output(trigger, False)
            start = time.time()
            while GPIO.input(echo) == 0:
                start = time.time()
            while GPIO.input(echo) == 1:
                stop = time.time()
            elapsed = stop - start
            distance = elapsed * 17000 #Why the hell I was multiplying by 34000 and dividing by 2 before....
            print distance
    else:
        print "Ending"
        running = False

#Cleanup before exit
GPIO.cleanup()
print "Done"