diff --git a/Control/Pi/proximity_detector.py b/Control/Pi/proximity_detector.py index dd75d0d..c903a84 100644 --- a/Control/Pi/proximity_detector.py +++ b/Control/Pi/proximity_detector.py @@ -1,13 +1,70 @@ import serial import logging import time +import os +import psutil +from threading import Thread + +class ProxStream: + def __init__(self): + self.log = logging.getLogger("PROXIMITY ARRAY LISTENER") + self.log.setLevel("DEBUG") + self.serial_string = '/dev/proximity' + try: + self.ser = serial.Serial(self.serial_string, 115200, timeout=1) + self.alive = True + self.log.info('Proximity serial open') + self.ser.flushInput() + except Exception as e: + self.log.critical(f"Cannot open serial device, {e}") + self.log.critical('Exiting') + current_system_pid = os.getpid() + ThisSystem = psutil.Process(current_system_pid) + ThisSystem.terminate() + self.proximity_buffer = [99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99] + + def start(self): + t = Thread(target=self.update, args=()) + t.daemon = True + self.running = True + t.start() + return self + + def update(self): + self.ser.flushInput() + while self.running: + try: + p1 = p2 = p3 = p4 = p5 = p6 = p7 = p8 = p9 = p10 = p11 = 0 + for _ in range(1): + raw = str(self.ser.readline()) + raw2 = raw.split("**")[1].split("##")[0].split("|") + raw3 = raw2 + p1 += int(raw3[0]) + p2 += int(raw3[1]) + p3 += int(raw3[2]) + p4 += int(raw3[3]) + p5 += int(raw3[4]) + p6 += int(raw3[5]) + p7 += int(raw3[6]) + p8 += int(raw3[7]) + p9 += int(raw3[8]) + p10 += int(raw3[9]) + p11 += int(raw3[10]) + self.proximity_buffer = [int(p1/2), int(p2/2), int(p3/2), int(p4/2), int(p5/2), int(p6/2), int(p7/32), int(p8/2), int(p9/2), int(p10/2), int(p11/2)] + except Exception as e: + self.log.critical(f"ERROR: {e}, serial input: {raw}") + + def read(self): + return self.proximity_buffer + + def stop(self): + self.running = False class Proximity: def __init__(self, state_machine, vehicle_ctrl) -> None: self.log = logging.getLogger("PROXIMITY ARRAY") self.log.setLevel("DEBUG") - # self.serial_string = '/dev/cu.usbmodem14601' self.serial_string = '/dev/proximity' self.alive = False self.proximity_stopped = False @@ -20,46 +77,12 @@ class Proximity: self.rear_sensors = [4, 5, 6, 7, 8, 9, 10] # TODO: change with more sensors self.proximity_detects = 0 self.proximity_detect_thresh = 1 - try: - self.ser = serial.Serial(self.serial_string, 9600, timeout=1) - self.alive = True - self.log.info('Proximity serial open') - self.ser.flushInput() - except Exception as e: - self.log.critical(f"Cannot open serial device, {e}") - self.log.critical('Exiting') - exit(0) + self.proximity_stream = ProxStream() self.proximities = [99, 99, 99, 99, 99, 99, 99, 99, 99, 99, 99] def get_prox_readings(self): - try: - # self.ser.flushInput() - # time.sleep(0.3) - p1 = p2 = p3 = p4 = p5 = p6 = p7 = p8 = p9 = p10 = p11 = 0 - for _ in range(1): - raw = str(self.ser.readline()) - raw2 = raw.split('\\r')[0].split("'")[1] - raw3 = raw2.split('|') - print(f"********* {raw3} *********") - p1 += int(raw3[0]) - p2 += int(raw3[1]) - p3 += int(raw3[2]) - p4 += int(raw3[3]) - p5 += int(raw3[4]) - p6 += int(raw3[5]) - p7 += int(raw3[6]) - p8 += int(raw3[7]) - p9 += int(raw3[8]) - p10 += int(raw3[9]) - p11 += int(raw3[10]) - self.proximities = [int(p1/2), int(p2/2), int(p3/2), int(p4/2), int(p5/2), int(p6/2), int(p7/32), int(p8/2), int(p9/2), int(p10/2), int(p11/2)] - self.failed_reads = 0 - return self.proximities - except Exception as e: - self.log.critical(f"ERROR: {e}, serial input: {raw}") - self.failed_reads += 1 - if self.failed_reads > self.failed_reads_thresh: - self.process_system_failure() + self.proximities = self.proximity_stream.read() + return self.proximities def process_system_failure(self): self.log.critical(f"Got {self.failed_reads} sequential fails, killing process...") diff --git a/Embedded/proximity_sensor/proximity_sensor.ino b/Embedded/proximity_sensor/proximity_sensor.ino index de70568..8b2f288 100644 --- a/Embedded/proximity_sensor/proximity_sensor.ino +++ b/Embedded/proximity_sensor/proximity_sensor.ino @@ -81,7 +81,7 @@ void setup() { pinMode(echo_pin_10, INPUT); pinMode(trig_pin_11, OUTPUT); pinMode(echo_pin_11, INPUT); - Serial.begin(9600); // Starts the serial communication + Serial.begin(115200); // Starts the serial communication } void pulse_pin(int trig_pin) { @@ -114,6 +114,7 @@ void loop() { distance_10 = get_dist(trig_pin_10, echo_pin_10); distance_11 = get_dist(trig_pin_11, echo_pin_11); + Serial.print("**"); Serial.print(distance_1); Serial.print(" | "); Serial.print(distance_2); @@ -134,5 +135,6 @@ void loop() { Serial.print(" | "); Serial.print(distance_10); Serial.print(" | "); - Serial.println(distance_11); + Serial.print(distance_11); + Serial.println("##"); }