|
<![CDATA[from gopigo import *
import time
enable_servo()
servo(90)
distance_to_stop=20
print "Press ENTER to start"
#Wait for input to start
raw_input()
fwd()
while True:
#Find the distance of the object in front
dist=us_dist(15)
print "Dist:",dist,'cm'
#If the object is closer than the "distance_to_stop" distance, stop the GoPiGo
if dist<distance_to_stop:
print "obstacle"
stop()
servo(20)
time.sleep(1)
dist=us_dist(15)
print "Right Dist:",dist,'cm'
a=dist
servo(160)
time.sleep(1)
dist=us_dist(15)
print "Left Dist:",dist,'cm'
b=dist
if a<=distance_to_stop and b<=distance_to_stop:
servo(90)
time.sleep(1.0)
bwd()
time.sleep(0.5)
stop()
print "I gave up and stop!"
#Stop the GoPiGo
break
elif a>b:
if a>distance_to_stop:
right_rot()
time.sleep(0.3)
stop()
servo(90)
time.sleep(1)
print "go right"
fwd()
elif b>a:
if b>distance_to_stop:
left_rot()
time.sleep(0.3)
stop()
servo(90)
time.sleep(1)
print "go left"
fwd()
time.sleep(0.1)
dist=us_dist(15)
time.sleep(.1)]]>
|