88 lines
2.0 KiB
Python
88 lines
2.0 KiB
Python
# Servo Control
|
|
import time
|
|
import wiringpi
|
|
import RPi.GPIO as GPIO
|
|
import os
|
|
|
|
GPIO.setmode(GPIO.BCM)
|
|
# use 'GPIO naming'
|
|
wiringpi.wiringPiSetupGpio()
|
|
|
|
# set #18 to be a PWM output
|
|
wiringpi.pinMode(18, wiringpi.GPIO.PWM_OUTPUT)
|
|
|
|
#set GPIO Pins
|
|
GPIO_TRIGGER = 23
|
|
GPIO_ECHO = 24
|
|
|
|
#set GPIO direction (IN / OUT)
|
|
GPIO.setup(GPIO_TRIGGER, GPIO.OUT)
|
|
GPIO.setup(GPIO_ECHO, GPIO.IN)
|
|
|
|
# set the PWM mode to milliseconds stype
|
|
wiringpi.pwmSetMode(wiringpi.GPIO.PWM_MODE_MS)
|
|
|
|
# divide down clock
|
|
wiringpi.pwmSetClock(192)
|
|
wiringpi.pwmSetRange(2000)
|
|
|
|
delay_period = 0.1
|
|
closed_position=50
|
|
opened_position=110
|
|
|
|
def distance():
|
|
# set Trigger to HIGH
|
|
GPIO.output(GPIO_TRIGGER, True)
|
|
|
|
# set Trigger after 0.01ms to LOW
|
|
time.sleep(0.00001)
|
|
GPIO.output(GPIO_TRIGGER, False)
|
|
|
|
StartTime = time.time()
|
|
StopTime = time.time()
|
|
|
|
# save StartTime
|
|
while GPIO.input(GPIO_ECHO) == 0:
|
|
StartTime = time.time()
|
|
|
|
# save time of arrival
|
|
while GPIO.input(GPIO_ECHO) == 1:
|
|
StopTime = time.time()
|
|
|
|
# time difference between start and arrival
|
|
TimeElapsed = StopTime - StartTime
|
|
# multiply with the sonic speed (34300 cm/s)
|
|
# and divide by 2, because there and back
|
|
distance = (TimeElapsed * 34300) / 2
|
|
|
|
return distance
|
|
|
|
if __name__ == '__main__':
|
|
try:
|
|
oldDist = 0
|
|
loopCounter = 0
|
|
while True:
|
|
dist = distance()
|
|
print('distance', dist)
|
|
|
|
if oldDist == 0:
|
|
oldDist = dist
|
|
print('1', dist - oldDist)
|
|
print('2', oldDist - dist)
|
|
# If the distance is below ~20 cm open the box
|
|
if dist < 20:
|
|
wiringpi.pwmWrite(18, opened_position)
|
|
time.sleep(10) # wait 10 seconds
|
|
wiringpi.pwmWrite(18, closed_position) # Close the lid
|
|
time.sleep(1)
|
|
|
|
# Reset by pressing CTRL + C
|
|
except KeyboardInterrupt:
|
|
print("Measurement stopped by User")
|
|
GPIO.cleanup()
|
|
|
|
|
|
#while True:
|
|
# wiringpi.pwmWrite(18, closed_position)
|
|
# exit()
|