compAIR/compLIB
compAIR/compLIB
Archived
4
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Merge remote-tracking branch 'origin/master'

Browse source
This commit is contained in:
Joel 2021-01-16 23:33:19 +01:00
commit 2cac75fc9c
No known key found for this signature in database
GPG key ID: BDDDBECD0808290E
2 changed files with 107 additions and 106 deletions
Download patch file Download diff file Expand all files Collapse all files

107
compLib/Ultrasonic.py Normal file
View file

@ -0,0 +1,107 @@
import time
import RPi.GPIO as GPIO
trigger_pin = 27
echo_pin = 22
speed_of_sound_mps = 340 # in m/s
rising_found = False
rising_time = 0
falling_found = False
falling_time = 0
class Ultrasonic:
@staticmethod
def __send_trigger():
GPIO.setup(trigger_pin, GPIO.OUT)
GPIO.output(trigger_pin, False) # Ensure a clean transition
time.sleep(0.000005)
GPIO.output(trigger_pin, True) # Trigger now
time.sleep(0.000010)
GPIO.output(trigger_pin, False)
@staticmethod
def __signal_start_callback(channel):
global rising_found
global rising_time
rising_found = True
rising_time = time.process_time_ns()
@staticmethod
def __signal_end_callback(channel):
global falling_found
global falling_time
falling_found = True
falling_time = time.process_time_ns()
@staticmethod
def __measure_pulse(timeout = 1000000) -> int:
""" Measure the length of a pulse in us. When either the pulse doesn't start in [timeout] us
or the pulse is longer than [timeout] us, 0 is returned.
:param timeout: Timeout in us
:return: The length of the pulse or 0 on timeout
:rtype: int
"""
global rising_found
global rising_time
global falling_found
global falling_time
# Setup pin
GPIO.setup(echo_pin, GPIO.IN)
# Wait for the rising edge
rising_found = False
GPIO.add_event_detect(echo_pin, GPIO.RISING, callback = Ultrasonic.__signal_start_callback)
start = time.process_time_ns()
while (time.process_time_ns() < start + timeout * 1000 and not rising_found):
pass
GPIO.remove_event_detect(echo_pin)
if (not rising_found):
return 0
# Wait for the falling edge
falling_found = False
GPIO.add_event_detect(echo_pin, GPIO.FALLING, callback = Ultrasonic.__signal_end_callback)
start = time.process_time_ns()
while (time.process_time_ns() < start + timeout * 1000 and not falling_found):
pass
GPIO.remove_event_detect(echo_pin)
if (not falling_found):
return 0
# Finally, return the time difference
return int((falling_time - rising_time) / 1000)
@staticmethod
def get_distance() -> float:
""" Get the distance from the Ultrasonic sensor in cm.
Returns 'NaN' when the result is invalid.
:return: The distance in cm or 'NaN'
:rtype: float
"""
GPIO.setmode(GPIO.BCM)
# Trigger the measurement
Ultrasonic.__send_trigger()
# The pulse length in us
pulse = Ultrasonic.__measure_pulse()
if (pulse == 0):
print("invalid")
return float('NaN')
# Convert the speed of sound from m/s to cm/us for convenience
speed_of_sound_cmpus = speed_of_sound_mps / 10000
# Convert us time to cm distance
distance = (pulse / 2) * speed_of_sound_cmpus
return distance

106
compLib/Ultrasound.py
View file

@ -1,106 +0,0 @@
import time
from Motor import *
import RPi.GPIO as GPIO
from servo import *
from PCA9685 import PCA9685
class Ultrasonic:
def __init__(self):
GPIO.setwarnings(False)
self.trigger_pin = 27
self.echo_pin = 22
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.trigger_pin, GPIO.OUT)
GPIO.setup(self.echo_pin, GPIO.IN)
def send_trigger_pulse(self):
GPIO.output(self.trigger_pin, True)
time.sleep(0.00015)
GPIO.output(self.trigger_pin, False)
def wait_for_echo(self, value, timeout):
count = timeout
while GPIO.input(self.echo_pin) != value and count > 0:
count = count - 1
def get_distance(self):
distance_cm = [0, 0, 0, 0, 0]
for i in range(3):
self.send_trigger_pulse()
self.wait_for_echo(True, 10000)
start = time.time()
self.wait_for_echo(False, 10000)
finish = time.time()
pulse_len = finish - start
distance_cm[i] = pulse_len / 0.000058
distance_cm = sorted(distance_cm)
return int(distance_cm[2])
def run_motor(self, L, M, R):
if (L < 30 and M < 30 and R < 30) or M < 30:
self.PWM.setMotorModel(-1450, -1450, -1450, -1450)
time.sleep(0.1)
if L < R:
self.PWM.setMotorModel(1450, 1450, -1450, -1450)
else:
self.PWM.setMotorModel(-1450, -1450, 1450, 1450)
elif L < 30 and M < 30:
PWM.setMotorModel(1500, 1500, -1500, -1500)
elif R < 30 and M < 30:
PWM.setMotorModel(-1500, -1500, 1500, 1500)
elif L < 20:
PWM.setMotorModel(2000, 2000, -500, -500)
if L < 10:
PWM.setMotorModel(1500, 1500, -1000, -1000)
elif R < 20:
PWM.setMotorModel(-500, -500, 2000, 2000)
if R < 10:
PWM.setMotorModel(-1500, -1500, 1500, 1500)
else:
self.PWM.setMotorModel(600, 600, 600, 600)
def run(self):
self.PWM = Motor()
self.pwm_S = Servo()
for i in range(30, 151, 60):
self.pwm_S.setServoPwm('0', i)
time.sleep(0.2)
if i == 30:
L = self.get_distance()
elif i == 90:
M = self.get_distance()
else:
R = self.get_distance()
while True:
for i in range(90, 30, -60):
self.pwm_S.setServoPwm('0', i)
time.sleep(0.2)
if i == 30:
L = self.get_distance()
elif i == 90:
M = self.get_distance()
else:
R = self.get_distance()
self.run_motor(L, M, R)
for i in range(30, 151, 60):
self.pwm_S.setServoPwm('0', i)
time.sleep(0.2)
if i == 30:
L = self.get_distance()
elif i == 90:
M = self.get_distance()
else:
R = self.get_distance()
self.run_motor(L, M, R)
ultrasonic = Ultrasonic()
# Main program logic follows:
if __name__ == '__main__':
print('Program is starting ... ')
try:
ultrasonic.run()
except KeyboardInterrupt: # When 'Ctrl+C' is pressed, the child program destroy() will be executed.
PWM.setMotorModel(0, 0, 0, 0)
ultrasonic.pwm_S.setServoPwm('0', 90)