$ source ./venv/bin/activate
(venv) $ vi obstacle_stop.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
time.sleep(0.3)
GPIO.output(TrigPin, GPIO.HIGH)
time.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
## 前進 ###############
# go forward 1sec
def go_fwd():
egpg.set_speed(100)
egpg.forward()
sleep(1)
egpg.stop()
## サーボ ###############
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 の値
Pan_Center = 1500
# 初期位置：正面に pan
pan(Pan_Center)
###### 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()
#Start moving
go_fwd()
try:
while True:
#Find the distance of the object in front
dist = dist_read()
sleep(1)
print("Dist:",dist,'cm')
#If the object is closer than distance_to_stop, stop the GoPiGo
if dist < distance_to_stop:
print("obstacle!")
egpg.stop()
bye()
sys.exit()
else:
go_fwd()
# except the program gets interrupted by Ctrl+C on the keyboard.
except KeyboardInterrupt:
egpg.stop()
bye()
]]>
|
(venv) $ chmod +x obstacle_stop.py
(venv) $ ./obstacle_stop.py
<![CDATA[Press ENTER to start
Dist: 93 cm
Dist: 88 cm
Dist: 84 cm
Dist: 80 cm
Dist: 76 cm
Dist: 70 cm
Dist: 66 cm
Dist: 63 cm
Dist: 56 cm
Dist: 52 cm
Dist: 51 cm
Dist: 41 cm
Dist: 36 cm
Dist: 32 cm
Dist: 26 cm
obstacle!
$
]]>
|