From 38ee4f9b9aa62af5bea2d27511c029f4ee288863 Mon Sep 17 00:00:00 2001
From: Cole Deck <cdeck@hawk.iit.edu>
Date: Tue, 14 May 2024 20:07:35 -0500
Subject: [PATCH] update sensor system

---
 run.py | 24 ++++++++++++------------
 1 file changed, 12 insertions(+), 12 deletions(-)

diff --git a/run.py b/run.py
index ab4f4e8..e61309d 100755
--- a/run.py
+++ b/run.py
@@ -460,7 +460,6 @@ def handle_error(error):
 def get_sensors():
     global mbconn
     global sensors
-    oldsens = sensors
     #print("Reading sensors")
     mbconn.open()
     """
@@ -474,27 +473,28 @@ def get_sensors():
     port 8: 368
     
     """
-    out = list()
     if real:
-        for reg in [352, 288, 304, 368]:
+        sens = [352, 288, 304, 368]
+        for idx in range(len(sens)):
+            reg = sens[idx]
             val = mbconn.read_holding_registers(reg)
             if val is not None:
                 val = val[0]
-                if val == 1:
-                    out.append(1)
+                if val == 1 and sensors[idx] >= 0: # skip negative values
+                    sensors[idx] += 1
                 else:
-                    out.append(0)
+                    sensors[idx] = 0
     else:
         out = [0, 0, 0, 0]
 
     if len(out) != 4:
         return -1
-    sensors = out
     #fprint("Values: " + str(sensors))
     mbconn.close()
-    for x in range(len(oldsens)):
-        if oldsens[x] == 0 and out[x] == 1:
+    for x in range(len(sensors)):
+        if sensors[x] >= 180: # 3 sec
             # cable newly detected on tray
+            sensors[x] = -1
             fprint("Precense detected: slot " + str(x))
             return x
         
@@ -771,10 +771,9 @@ def mainloop_server(pool, manager):
             global mainloop_get
             #print("Checking sensors..")
             newtube = get_sensors()
-            if newtube >= 0 and newtube != just_placed:
+            if newtube >= 0:
                 # need to return a cable
                 mainloop_get.put(("return", newtube))
-                just_placed = -1
             
             if not mainloop_get.empty():
                 fprint("Movement requested. Keep clear of the machine!")
@@ -796,7 +795,7 @@ def mainloop_server(pool, manager):
                             fprint("Getting cable at position " + str(get_cable))
                             mode = "Pickup"
                             cable_list_state[get_cable] = False # mark as removed
-                            get_sensors()
+                            
 
                     if action == "return":
                         arm_ready = False
@@ -904,6 +903,7 @@ def mainloop_server(pool, manager):
         else: # camera not ready
             timecount += 1
             if timecount > 180:
+                print("Camera timeout reached.")
                 timecount = 0
                 camera_ready = True
                 arm_ready = True