From 8bc35858fb192fd8f18454754647cf5ef6360376 Mon Sep 17 00:00:00 2001 From: HerrNamenlos123 Date: Sat, 16 Jan 2021 22:40:33 +0100 Subject: [PATCH] Added Ultrasonic sensor --- compLib/Ultrasonic.py | 107 ++++++++++++++++++++++++++++++++++++++++++ compLib/Ultrasound.py | 106 ----------------------------------------- 2 files changed, 107 insertions(+), 106 deletions(-) create mode 100644 compLib/Ultrasonic.py delete mode 100644 compLib/Ultrasound.py diff --git a/compLib/Ultrasonic.py b/compLib/Ultrasonic.py new file mode 100644 index 0000000..2ecbf60 --- /dev/null +++ b/compLib/Ultrasonic.py @@ -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 diff --git a/compLib/Ultrasound.py b/compLib/Ultrasound.py deleted file mode 100644 index 6cfbd89..0000000 --- a/compLib/Ultrasound.py +++ /dev/null @@ -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) \ No newline at end of file