obstacle_avoidance.py
|
<![CDATA[#!/usr/bin/env python
import gopigo3
from easygopigo3 import EasyGoPiGo3
from di_sensors.easy_distance_sensor import EasyDistanceSensor
from time import sleep
import sys
gpg = gopigo3.GoPiGo3()
egpg = EasyGoPiGo3()
# Servo
# The case the servo cable is connected to Servo2.
servo_n = gpg.SERVO_2
# Distance sensor faces the front when gpg.set_servo(servo_n, 1400)
servo_center = 1400
def servo_front():
gpg.set_servo(servo_n, servo_center)
def servo_right():
gpg.set_servo(servo_n, servo_center - 500)
def servo_left():
gpg.set_servo(servo_n, servo_center + 500)
# Distance Sensor
dist_sensor = EasyDistanceSensor()
#Distance from obstacle where the GoPiGo should stop : 30cm
distance_to_stop = 30
# Moving
# go forward 1sec
def go_fwd():
egpg.set_speed(100)
egpg.forward()
sleep(1)
egpg.stop()
# go backward 1sec
def go_bwd():
egpg.set_speed(100)
egpg.backward()
sleep(1)
egpg.stop()
# turn right 90deg (2.5sec)
def turn_right():
egpg.set_speed(50)
egpg.right()
sleep(2.5)
egpg.stop()
# turn left 90deg (2.5sec)
def turn_left():
egpg.set_speed(50)
egpg.left()
sleep(2.5)
egpg.stop()
# Exit
def destroy():
gpg.reset_all()
egpg.reset_all()
sys.exit()
#### The Program starts from here ###############
print("Press ENTER to start")
#Wait for input to start
input()
#Start moving
servo_front()
go_fwd()
try:
while True:
#Find the distance of the object in front
dist = dist_sensor.read()
sleep(1)
#print("distance from object: {} cm".format(dist))
print("Dist:",dist,'cm')
#If the object is closer than distance_to_stop, stop the GoPiGo
if dist < distance_to_stop:
print("obstacle!")
servo_right()
dist = dist_sensor.read()
sleep(1)
print("Right Dist:",dist,'cm')
r_dist = dist
servo_left()
dist = dist_sensor.read()
sleep(1)
print("Left Dist:",dist,'cm')
l_dist = dist
servo_front()
sleep(1)
# r_dist, l_dist <= distance_to_stop
if r_dist<=distance_to_stop and l_dist<=distance_to_stop:
go_bwd()
print("I gave up and stop!")
destroy()
# r_dist or l_dist > distance_to_stop
else:
if r_dist>=l_dist:
turn_right()
print("go right")
go_fwd()
else:
turn_left()
print("go left")
go_fwd()
else:
go_fwd()
# except the program gets interrupted by Ctrl+C on the keyboard.
except KeyboardInterrupt:
egpg.stop()
destroy()
]]>
|
$ vi obstacle_avoidance.py
$ chmod +x obstacle_avoidance.py
$ ./obstacle_avoidance.py
|