Update

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...")

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("##");
 }