|
<![CDATA[#!/usr/bin/python
###### Sensor ####################################
import RPi.GPIO as GPIO
# Set the GPIO pins as numbering
GPIO.setmode(GPIO.BOARD)
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)
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)
#Distance from obstacle where the GoPiGo should stop : 30cm
distance_to_stop = 30
###### motor #############################
from easygopigo3 import EasyGoPiGo3
egpg = EasyGoPiGo3()
# Moving
# go forward 1sec
def go_fwd():
egpg.set_speed(100)
egpg.forward()
time.sleep(1)
egpg.stop()
###### exit process #############################
def bye():
egpg.stop()
# Release resource
GPIO.cleanup()
egpg.reset_all()
#### The Program starts from here ###############
# Wait for input to start
input('Press ENTER to start')
#Start moving
go_fwd()
while True:
try:
dist = dist_read()
if( dist < 450 ):
print(dist,'cm')
if dist > distance_to_stop:
go_fwd()
else:
print('obstacle!')
break
else:
print('? ',dist,'cm')
break
except KeyboardInterrupt:
break
# exit
bye()]]>
|
