diff --git a/parking_sensor.py b/parking_sensor.py new file mode 100644 index 0000000000000000000000000000000000000000..66d7909a8b364549679e4e0d36730f10a9e9dc0f --- /dev/null +++ b/parking_sensor.py @@ -0,0 +1,127 @@ +import time +import datetime +import sys +import os +import grovepi +import math +import json +import traceback +import subprocess +import multiprocessing as mp +from video_server import runVideo + + +ranger = 4 +buzzer = 6 + +alertThreshold = 100 +midThreshold = 50 +warningThreshold = 20 + +grovepi.pinMode(ranger, "INPUT") +grovepi.pinMode(buzzer, "OUTPUT") + + +startBuzz = None +buzzDelay = None +buzzOn = False +LCDUpdated = datetime.datetime.now().timestamp() +buzzDuration = 0.05 + + +#Read data from ultrasonic ranger +def readDistance(): + try: + distance = grovepi.ultrasonicRead(ranger) + return distance + except: + return None + + +def setBuzzer(buzzOn): + buzzing = grovepi.digitalRead(buzzer) + # Read buzzer state, if buzz is needed and not buzzing already, then start buzzing. Turn off if buzz state is not wanted. + if buzzOn: + if not buzzing: + grovepi.digitalWrite(buzzer, 1) + buzzing = True + else: + if buzzing: + grovepi.digitalWrite(buzzer, 0) + buzzing = False + +#Wrapper for the buzzer +def buzzerWrapper(distance): + global startBuzz, buzzDelay, buzzOn, buzzDuration + #Only start buzzer if in alert threshold + if distance and distance <= alertThreshold: + #Delay is equal to inverse of distance, with a minimum value + delay = (distance-warningThreshold)/100 + delay = max(0.03, delay) + #If buzzer has not started, start it and set time. + if startBuzz is None: + setBuzzer(True) + startBuzz = datetime.datetime.now().timestamp() + #If not in delay, but are buzzing, keep buzzing until buzz duration has elapsed. + elif not buzzDelay and startBuzz and datetime.datetime.now().timestamp() - startBuzz > buzzDuration: + if distance > warningThreshold: + buzzDelay = datetime.datetime.now().timestamp() + setBuzzer(False) + #If in delay, delay until the pause between delays has elapsed. + elif buzzDelay and datetime.datetime.now().timestamp() - buzzDelay > delay: + buzzDelay = None + startBuzz = None + + else: + #If not in alert threshold, make sure buzzer is off. + setBuzzer(False) + +def resetSystem(): + #Reset the system to off state + grovepi.digitalWrite(buzzer, 0) + + +def videoProcess(): + queue = mp.Queue() + return mp.Process(target=runVideo, args=(queue,)), queue + + +def exceptHandler(process, queue): + #Force system quit on exception + if queue: + queue.put('terminate') + time.sleep(1) + if process: + process.terminate() + process.join() + resetSystem() + exit() + +if __name__ == '__main__': + process = None + try: + #Start the video server + process, queue = videoProcess() + process.start() + + #Initialise multiprocessing dictionary + manager = mp.Manager() + d = manager.dict() + d['warningThreshold'] = warningThreshold + d['midThreshold'] = midThreshold + d['alertThreshold'] = alertThreshold + + #First read of distance is None + readDistance() + while True: + #Event loop + distance = readDistance() + d['distance'] = distance + queue.put(d) + buzzerWrapper(distance) + except KeyboardInterrupt: + exceptHandler(process, queue) + except Exception as e: + print(e) + exceptHandler(process, queue) +