Merge branch 'master' into transport

Former-commit-id: 8a0e3cae8b38d79badc1c6bc8184bc2c689454a5

Auto_Ctrl/predictor_Syringe_Pump.py

@@ -12,6 +12,7 @@ import joblib
 import json
 import Find_COM
 from threading import Thread
+import atexit
 
 LOCAL_DEBUG = False
 
@@ -20,7 +21,7 @@ if LOCAL_DEBUG:
     time.sleep(2)
 
 class MAT:
-    def __init__(self, videoSourceIndex=0, weights_path = "resnet34-1Net.pth", json_path = 'class_indices.json', classes = 2):
+    def __init__(self, videoSourceIndex=0, weights_path = "resnet34-1Net.pth", json_path = 'class_indices.json', classes = 2,bounce_time=1):
         print('实验初始化中')
         self.data_root = os.getcwd()
         self.videoSourceIndex = videoSourceIndex  # 摄像机编号
@@ -30,6 +31,7 @@ class MAT:
             self.port = Find_COM.list_ch340_ports()[0]  # 串口名
             self.pump_ser = serial.Serial(self.port, 9600) # 初始化串口
         self.classes = classes
+        self.bounce_time = bounce_time # 防抖时间
         self.total_volume = 0 # 记录总体积
         self.now_volume = 0 # 记录当前注射泵内体积
         self.volume_list = [] # 记录体积变化
@@ -41,11 +43,13 @@ class MAT:
         self.formatted_time = datetime.fromtimestamp(self.start_time).strftime('%Y%m%d_%H%M%S')
 
         self.model = joblib.load("model.pkl")
+        atexit.register(self.start_move_3)
         print("实验开始于", self.formatted_time)
 
     def start_move_1(self): # 抽料程序
         if LOCAL_DEBUG:return
-        data = b"q1h24d" # *2
+        self.start_move_init()
+        data = b"q1h40d" # *2
         self.pump_ser.write(data)
         time.sleep(0.01)
         data = b"q2h0d"
@@ -54,14 +58,44 @@ class MAT:
         data = b"q4h0d"
         self.pump_ser.write(data)
         time.sleep(0.01)
-        data = b"q5h15d"
+        data = b"q5h9d"
         self.pump_ser.write(data)
         time.sleep(0.01)
         data = b"q6h3d"
         self.pump_ser.write(data)
-        time.sleep(15)
+        time.sleep(9)
         print('完成抽取')
 
+    def start_move_init(self): # init
+        if LOCAL_DEBUG:return
+        data = b"q1h15d" # *2
+        self.pump_ser.write(data)
+        time.sleep(0.01)
+        data = b"q2h0d"
+        self.pump_ser.write(data)
+        time.sleep(0.01)
+        data = b"q4h0d"
+        self.pump_ser.write(data)
+        time.sleep(0.01)
+        data = b"q5h2d"
+        self.pump_ser.write(data)
+        time.sleep(0.01)
+        data = b"q6h2d"
+        self.pump_ser.write(data)
+        print("send1")
+        time.sleep(2)
+        data = b"q1h20d" # *2
+        self.pump_ser.write(data)
+        time.sleep(0.01)
+        data = b"q5h1d"
+        self.pump_ser.write(data)
+        time.sleep(0.1)
+        data = b"q6h3d"
+        self.pump_ser.write(data)
+        print("send2")
+        time.sleep(1)
+        print('INITED')
+
     def start_move_2(self, speed=0.1): # 进料程序
         if LOCAL_DEBUG:
             time.sleep(1)
@@ -92,20 +126,23 @@ class MAT:
         if LOCAL_DEBUG:return
         data = b"q6h6d"
         self.pump_ser.write(data)
-
+    
     def preproc(self, im):
         try:
             hsv = cv2.cvtColor(im,cv2.COLOR_BGR2HSV)
             mask = hsv[:,:,1] > 150
             mask = mask[:,:,np.newaxis]
             cnt = np.count_nonzero(mask)
+            if cnt == 0:
+                return 0,0,0
             hsv*=mask
             h = round(np.sum(hsv[:,:,0])/cnt)
             s = round(np.sum(hsv[:,:,1])/cnt)
             v = round(np.sum(hsv[:,:,2])/cnt)
             return h,s,v
         except Exception as e :
-            print(e)
+            import traceback
+            traceback.print_exc()
             return None
         # name = f"{cl}_{h}_{s}_{v}.jpg"
 
@@ -118,15 +155,45 @@ class MAT:
         ret = self.my_predictor(im)
         # print(ret)
         if ret is None:
-            print("Failed")
+            print("Fallback")
             self.thr = Thread(target=self._pred).start()
         else:
-            if ret == self.end_kind:
+            # fps
-                print("Stop at ",self.total_volume)
+            now = time.time()
-                self.running = False
+            if now - self.last[0] > 1:
-                self.start_move_3()
+                print("FPS: ",self.last[1])
+                print(ret)
+                self.last[0] = now
+                self.last[1] = 0
             else:
-                self.thr = Thread(target=self._pred).start()
+                self.last[1] += 1
+            # fps end
+            now = time.time()
+        
+            if ret == self.end_kind:
+                if self.debounce[0]:
+                    if self.debounce[1] and self.debounce[0]:
+                        # print(self.debounce)
+                        if now-self.debounce[0][-1] > self.bounce_time:
+                            print("Bounce check succeeded, val:",self.debounce[1][0])
+                            self.running = False
+                            self.start_move_3()
+                            return
+                    else:
+                        print("Got stop flag, bounce check start, val:",self.total_volume)
+                    self.debounce[1].append((time.time(),self.total_volume))
+            else:
+                if self.debounce[0]:
+                    # print(self.debounce)
+                    if self.debounce[1]:
+                        # print(self.debounce[1][0][0],now,self.bounce_time)
+                        # print(self.debounce[1][0][0] > now - self.bounce_time)
+                        while self.debounce[1] and self.debounce[1][0][0] > now - self.bounce_time:
+                            self.debounce[1].pop(0)
+                    while self.debounce[0] and self.debounce[0][0] < now - self.bounce_time:
+                        self.debounce[0].pop(0)
+                self.debounce[0].append(now)
+            self.thr = Thread(target=self._pred).start()
             return ret,0.9
 
     def my_predictor(self,im):
@@ -190,10 +257,11 @@ class MAT:
         if not cv2.imwrite(name,im):
             print("Failed to save image",name)
 
-    def run(self,quick_speed = 0.2, mid_speed=0.1,slow_speed = 0.05,expect = 5, end_kind = 'orange'):
+    def run(self,quick_speed = 0.2, mid_speed=0.1,slow_speed = 0.05,expect:float|int = 5, end_kind = 'orange'):
         self.running = True
         self.end_kind = end_kind
-        self.cnt = 0
+        self.last = [time.time(),0]
+        self.debounce = [[],[]]
         self.thr = Thread(target=self._pred)
         self.thr.start()
         switching_point = expect * 0.9
@@ -235,7 +303,15 @@ if __name__ == "__main__":
     warnings.filterwarnings('ignore')
 
     # 创建MAT类的实例并运行
-    mat = MAT(videoSourceIndex = 1, weights_path = "resnet34-1Net.pth", json_path = 'class_indices.json', classes = 2)
+    mat = MAT(
+        videoSourceIndex = 1, 
+        weights_path = "resnet34-1Net.pth", 
+        json_path = 'class_indices.json', 
+        classes = 2,
+        bounce_time=1
+    )
+    
+    # exit()
     mat.run(
         quick_speed = 0.3, 
         slow_speed = 0.2,