$ source ./venv/bin/activate
(venv) $ vi obstacle_avoidance.py
|
<![CDATA[#!/usr/bin/env python
###### GPIO ####################################
import RPi.GPIO as GPIO
# Set the GPIO pins as numbering
GPIO.setmode(GPIO.BOARD)
###### Sensor ####################################
TrigPin = 22
EchoPin = 18
# Set the TrigPin's mode is output
GPIO.setup(TrigPin,GPIO.OUT)
GPIO.output(TrigPin, GPIO.LOW)
# Set the EchoPin's mode is input, and ON→ HIGH
GPIO.setup(EchoPin, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
###### Distance from obstacle where GoPiGo should stop : 30cm
distance_to_stop = 30
###### 距離計測 ####################################
import time
def dist_read():
# 10us pulse to TrigPin
sleep(0.3)
GPIO.output(TrigPin, GPIO.HIGH)
sleep(0.00001)
GPIO.output(TrigPin, GPIO.LOW)
# 超音波発信
# EchoPin が LOW から HIGH に変わる時刻 signaloff
while GPIO.input(EchoPin) == GPIO.LOW:
signaloff = time.time()
# EchoPin が HIGH から LOW に変わる時刻 signalon
while GPIO.input(EchoPin) == GPIO.HIGH:
signalon = time.time()
# EchoPin が HIGH だった時間
timepassed = signalon - signaloff
distance = timepassed * 17000
return round(distance)
##### GoPiGo motor ##########################
from easygopigo3 import EasyGoPiGo3
egpg = EasyGoPiGo3()
from time import sleep
import gopigo3
gpg = gopigo3.GoPiGo3()
## 前進・後退 ###############
# go forward
def go_fwd(dist):
egpg.set_speed(100)
egpg.drive_cm(dist, True)
# go backward 2sec
def go_bk():
egpg.set_speed(100)
egpg.backward()
sleep(2)
egpg.stop()
## 回転 ####################
def rotation(t):
egpg.set_speed(50)
if t > 0:
egpg.left()
sleep(t)
else:
egpg.right()
sleep(-t)
egpg.stop()
# 左45°回転の時間
Rot45 = 3.8
# 左90°回転の時間
Rot90 = 7.5
# 左120°回転の時間
Rot120 = 10.5
# turn right 90deg
def turn_right():
print('turn right\n')
rotation(-Rot90)
# turn left 90deg
def turn_left():
print('turn left\n')
rotation(Rot90)
## サーボ ###############
import gopigo3
gpg = gopigo3.GoPiGo3()
# The case the servo cable is connected to Servo1.
servo_n = gpg.SERVO_1
def pan(t):
gpg.set_servo(servo_n, t )
# 正面 pan の値
Pan0 = 1500
# ＋45° pan の値
Pan45 = 400
def pan_front():
pan(Pan0)
def pan_right():
pan(Pan0 - 500)
def pan_left():
pan(Pan0 + 500)
# 初期位置：正面に pan
pan_front()
###### exit process #############################
import sys
# Exit
def bye():
# Release resource
GPIO.cleanup()
egpg.reset_all()
gpg.reset_all()
#プログラムを終了
sys.exit()
#### The Program starts from here ###############
print("Press ENTER to start")
#Wait for input to start
input()
try:
while True:
#Find the distance of the object in front
dist = dist_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!\n")
pan_right()
dist = dist_read()
sleep(1)
print("Right Dist:",dist,'cm')
r_dist = dist
pan_left()
dist = dist_read()
sleep(1)
print("Left Dist:",dist,'cm')
l_dist = dist
pan_front()
sleep(1)
# r_dist, l_dist <= distance_to_stop
if r_dist<=distance_to_stop and l_dist<=distance_to_stop:
go_bk()
print("I gave up and stop!")
bye()
# r_dist or l_dist > distance_to_stop
else:
if r_dist>=l_dist:
turn_right()
else:
turn_left()
else:
go_fwd( round(dist/3) )
# except the program gets interrupted by Ctrl+C on the keyboard.
except KeyboardInterrupt:
egpg.stop()
bye()
]]>
|
(venv) $ chmod +x obstacle_avoidance.py
(venv) $ ./obstacle_avoidance.py
<![CDATA[Press ENTER to start
Dist: 100 cm
Dist: 72 cm
Dist: 53 cm
Dist: 37 cm
Dist: 27 cm
obstacle!
Right Dist: 34 cm
Left Dist: 42 cm
turn left
Dist: 96 cm
Dist: 75 cm
Dist: 86 cm
Dist: 84 cm
Dist: 108 cm
Dist: 120 cm
Dist: 16 cm
obstacle!
Right Dist: 18 cm
Left Dist: 44 cm
turn left
Dist: 204 cm
Dist: 81 cm
^C
(venv) $
]]>
|