removed calibration thread

raspberry-pi/config.ini

@@ -26,7 +26,8 @@
 
   # default arduino timing overhead in us
   # only used before calibration
-  overhead = 150
+  overhead_left  = 150
+  overhead_right = 100
 
 [mag_sensor]
 

raspberry-pi/connection.py

@@ -87,8 +87,6 @@ class ArduinoSlave(SerialConnection):
 
   def close(self):
     super().close()
-    if self._t.is_alive():
-      self._t._stop()
 
   def getAcusticRTTs(self): # in microseconds
     return [

raspberry-pi/gui/mainWindow.py

@@ -22,7 +22,7 @@ class MainWindow(tk.Frame):
     l.pack(side="top", fill="both", expand=True)
     calibrate_button = tk.Button(self.controls,text="calibrate",command=self.calibrate)
     calibrate_button.pack(side="top")
-    calibrate_button_next = tk.Button(self.controls,text="calibrate_next",command=self.calibration_state.next_state)
+    calibrate_button_next = tk.Button(self.controls,text="calibrate_next",command=self.calibration_state.next_state_gui)
     calibrate_button_next.pack(side="top")
 
     self.csString = tk.StringVar()
@@ -35,8 +35,6 @@ class MainWindow(tk.Frame):
     self.root.after(30, self.update)
 
   def calibrate(self):
-    self.calibration_state.reset_state()
-    self.calibration_state.next_state()
     self.down_queue.put("calibrate")
 
 if __name__ == "__main__":

raspberry-pi/main.py

@@ -12,80 +12,69 @@ conf.read('config.ini')
 
 class CalibrationStateMashine():
 
-    def __init__(self):
-        self.state = 0
-        self.value_count = 0
-
-        self.NOT_CALIBRATED = 0
-        self.READY_ON_POS_1 = 1
-        self.ACCUMULATING_1 = 2
-        self.FINISHED_POS_1 = 3
-        self.READY_ON_POS_2 = 4
-        self.ACCUMULATING_2 = 5
-        self.FINISHED_POS_2 = 6
-        self.CALIBRATION_DONE = 7
-
-    def state_clearname(self):
-        if self.state == self.NOT_CALIBRATED:
-            return "not calibrated"
-        elif self.state == self.READY_ON_POS_1:
-            return "ready on position one"
-        elif self.state == self.ACCUMULATING_1:
-            return "gathering values on position one"
-        elif self.state == self.FINISHED_POS_1:
-            return "finished gathering values from position one"
-        elif self.state == self.READY_ON_POS_2:
-            return "ready on position two"
-        elif self.state == self.ACCUMULATING_2:
-            return "gathering values on position two"
-        elif self.state == self.FINISHED_POS_2:
-            return "calculating calibration values"
-        elif self.state == self.CALIBRATION_DONE:
-            return "calibration done"
+  def __init__(self):
+    self.state = 0
+    self.value_count = 0
+
+    self.NOT_CALIBRATED = 0
+    self.WAITING_POS_1  = 1
+    self.ACCUMULATING_1 = 2
+    self.WAITING_POS_2  = 3
+    self.ACCUMULATING_2 = 4
+    self.CALIBRATION_DONE = 5
+
+  def state_clearname(self):
+    if self.state == self.NOT_CALIBRATED:
+      return "not calibrated"
+    elif self.state == self.WAITING_POS_1:
+      return "ready on position one"
+    elif self.state == self.ACCUMULATING_1:
+      return "gathering values on position one"
+    elif self.state == self.WAITING_POS_2:
+      return "ready on position two"
+    elif self.state == self.ACCUMULATING_2:
+      return "gathering values on position two"
+    elif self.state == self.CALIBRATION_DONE:
+      return "calibration done"
     
-    def next_state(self):
-        if self.state < 7:
-            self.state += 1
-            print(self.state_clearname())
+  def next_state(self):
+    if self.state < self.CALIBRATION_DONE:
+      self.state += 1
+      print(self.state_clearname())
 
-    def add_value(self):
-        self.value_count += 1
-    
-    def return_state(self):
-        return self.state
+  def next_state_gui(self):
+    if self.state == self.WAITING_POS_1 or self.state == self.WAITING_POS_2:
+      self.next_state()
 
-    def return_value_count(self):
-        return self.value_count
+  def get_state(self):
+    return self.state
 
-    def reset_state(self):
-        self.state = 0
-    
-    def reset_value_count(self):
-        self.value_count = 0
+  def reset_state(self):
+    self.state = 0
 
 
 def main():
-    up_queue = queue.Queue()
-    down_queue = queue.Queue()
-    calibration_state = CalibrationStateMashine()
-    ac_sensor = sensors.AcusticSensor(conf,up_queue,down_queue,calibration_state)
-    sensor_thread = threading.Thread(target=ac_sensor.start)
-
-    try:
-        sensor_thread.start()
-        root = tk.Tk()
-        root.title("Tracking System")
-        view = MainWindow(root,up_queue,down_queue,calibration_state)
-        view.pack(side="top", fill="both", expand=True)
-        view.update()
-        root.mainloop()
-
-    except KeyboardInterrupt:
-        print("stop")
-    except Exception as e:
-        print("Error: ",e)
-    finally:
-        down_queue.put("stop")
+  up_queue = queue.Queue()
+  down_queue = queue.Queue()
+  calibration_state = CalibrationStateMashine()
+  ac_sensor = sensors.AcusticSensor(conf,up_queue,down_queue,calibration_state)
+  sensor_thread = threading.Thread(target=ac_sensor.start)
+
+  try:
+    sensor_thread.start()
+    root = tk.Tk()
+    root.title("Tracking System")
+    view = MainWindow(root,up_queue,down_queue,calibration_state)
+    view.pack(side="top", fill="both", expand=True)
+    view.update()
+    root.mainloop()
+
+  except KeyboardInterrupt:
+    print("stop")
+  except Exception as e:
+    print("Error: ",e)
+  finally:
+    down_queue.put("stop")
 
 main()
 

raspberry-pi/sensors.py

@@ -19,6 +19,12 @@ class AcusticSensor:
     self.conf = conf
     self.ac_conf = conf["ac_sensor"]
 
+    self.time_vals = [[],[]]
+    self.calib_measurements = {
+      "left": [0, 0],
+      "right": [0, 0] 
+    }
+
   def start(self):
     self.running = True
     if not conn.isConnected():
@@ -30,13 +36,13 @@ class AcusticSensor:
       action = self.down_queue.get()
       print("action",action)
       if action == "calibrate":
-        calibration_thread = threading.Thread(target= self.calibrate)
-        calibration_thread.start()
+        self.calibration_state.reset_state()
+        self.time_vals = [[],[]]
+        self.calibration_state.next_state()
       elif action == "stop":
         print("exit Sensor")
         self.running = False
         thread.join()
-        calibration_thread.join()
         break
     conn.close()
 
@@ -44,97 +50,65 @@ class AcusticSensor:
     while self.running:
       value = (900+random.randint(0,300),900+random.randint(0,300))
       #print("dummy acc: ", value)
-      if self.calibration_state.return_state() == 2: #first range of calibration values
+      if self.calibration_state.get_state() == self.calibration_state.ACCUMULATING_1:
         value = (1541+random.randint(-50,50),2076+random.randint(-50,50))
-        self.time_vals[0].append(value[0])
-        self.time_vals[1].append(value[1])
-        self.calibration_state.add_value()
 
-      elif self.calibration_state.return_state() == 5: #second range of calibration values
+      elif self.calibration_state.get_state() == self.calibration_state.ACCUMULATING_2:
         value = (2076+random.randint(-50,50),1541+random.randint(-50,50))
-        self.time_vals[0].append(value[0])
-        self.time_vals[1].append(value[1])
-        self.calibration_state.add_value()
 
-      else:
-        self.pass_to_gui(self.calculate_position(value))
+      self.calibrate(value)
+      self.pass_to_gui(self.calculate_position(value))
       time.sleep(0.01)
 
   def _readCb(self, raw):
     value = conn.getAcusticRTTs()
     print("acc: ", value)
-    if self.calibration_state.return_state() == 2: #first range of calibration values
-        self.time_vals[0].append(value[0])
-        self.time_vals[1].append(value[1])
-        self.calibration_state.add_value()
-
-    elif self.calibration_state.return_state() == 5: #second range of calibration values
-        self.time_vals[0].append(value[0])
-        self.time_vals[1].append(value[1])
-        self.calibration_state.add_value()
-
-    else:
-      self.pass_to_gui(self.calculate_position(value))
 
-  def calibrate(self):
-    if not self.calibration_state.return_state() == 1:
-      print("current calibration state:",self.calibration_state.return_state(),"need to be 1 to start calibration!")
-      return
-    else:
-      print("start calibration")
-    self.time_vals = [[],[]]
-    self.calibration_state.next_state()
-    while self.calibration_state.return_value_count() < 100:
-      print("value count",self.calibration_state.return_value_count())
-      time.sleep(1) # wait until 100 values are gathered
-      #todo add timeout
-      if not self.running:
-        self.calibration_state.reset_state()
-        return
-    left_time_1 = statistics.mean(self.time_vals[0])
-    right_time_2 = statistics.mean(self.time_vals[1])
-    self.calibration_state.next_state() # signal gui to get next position
-
-    while self.calibration_state.return_state() != 4:
-      time.sleep(1) # wait till ui is ready for second position
-      #todo add timeout
-      if not self.running:
-        self.calibration_state.reset_state()
-        return
-
-    self.time_vals = [[],[]]
-    self.calibration_state.reset_value_count()
-    self.calibration_state.next_state()
-    while self.calibration_state.return_value_count() < 100:
-      print("value count",self.calibration_state.return_value_count())
-      time.sleep(1) # wait until 100 values are gathered
-      #todo add timeout
-      if not self.running:
-        self.calibration_state.reset_state()
-        return
-    left_time_2 = statistics.mean(self.time_vals[0])
-    right_time_1 = statistics.mean(self.time_vals[1])
-    self.calibration_state.next_state() # signal gui start of calculation
-
-    timedif = left_time_2 - left_time_1
-    distance_1 = math.sqrt(float(self.ac_conf["left_x_offset"])**2 + (float(self.ac_conf["y_offset"]) + float(self.conf["field"]["y"]))**2 )
-    distance_2 = math.sqrt((float(self.ac_conf["left_x_offset"]) + float(self.conf["field"]["x"])**2 + (float(self.ac_conf["y_offset"]) + float(self.conf["field"]["y"])))**2 )
-    distancedif = distance_2 - distance_1
-    sonicspeed_1 = distancedif / timedif
-    overhead_1 = statistics.mean((left_time_1 - distance_1/sonicspeed_1,left_time_2 - distance_2/sonicspeed_1))
-    print(left_time_1,distance_1,sonicspeed_1,left_time_2,distance_2,sonicspeed_1)
-
-    timedif = right_time_2 - right_time_1
-    distance_1 = math.sqrt(float(self.ac_conf["right_x_offset"])**2 + (float(self.ac_conf["y_offset"]) + float(self.conf["field"]["y"]))**2 )
-    distance_2 = math.sqrt((float(self.ac_conf["right_x_offset"]) + float(self.conf["field"]["x"])**2 + (float(self.ac_conf["y_offset"]) + float(self.conf["field"]["y"])))**2 )
-    distancedif = distance_2 - distance_1
-    sonicspeed_2 = distancedif / timedif
-    overhead_2 = statistics.mean((right_time_1 - distance_1/sonicspeed_2,right_time_2 - distance_2/sonicspeed_2))
-
-    self.ac_conf["sonicspeed"] = str(statistics.mean((sonicspeed_1,sonicspeed_2)))
-    self.ac_conf["overhead"]   = str(statistics.mean((overhead_1,overhead_2)))
-    print("calibration result",float(self.ac_conf["sonicspeed"]),float(self.ac_conf["overhead"]))
-    self.calibration_state.next_state()
+    self.calibrate(value)
+    self.pass_to_gui(self.calculate_position(value))
+
+  def calibrate(self, value):
+
+    if self.calibration_state.get_state() == self.calibration_state.ACCUMULATING_1:
+      self.time_vals[0].append(value[0])
+      self.time_vals[1].append(value[1])
+      if len(self.time_vals[0]) >= 100:
+        self.calib_measurements["left"][0]  = statistics.mean(self.time_vals[0])
+        self.calib_measurements["right"][1] = statistics.mean(self.time_vals[1])
+        self.time_vals = [[],[]]
+        self.calibration_state.next_state() # signal gui to get next position
+
+    elif self.calibration_state.get_state() == self.calibration_state.ACCUMULATING_2:
+      self.time_vals[0].append(value[0])
+      self.time_vals[1].append(value[1])
+      if len(self.time_vals[0]) >= 100:
+        self.calib_measurements["left"][1]  = statistics.mean(self.time_vals[0])
+        self.calib_measurements["right"][0] = statistics.mean(self.time_vals[1])
+
+        # all values have been captured
+        print(self.calib_measurements)
+        
+        # calculate calibration results
+        timedif = self.calib_measurements["left"][1] - self.calib_measurements["left"][0]
+        distance_1 = math.sqrt(float(self.ac_conf["left_x_offset"])**2 + (float(self.ac_conf["y_offset"]) + float(self.conf["field"]["y"]))**2 )
+        distance_2 = math.sqrt((float(self.ac_conf["left_x_offset"]) + float(self.conf["field"]["x"])**2 + (float(self.ac_conf["y_offset"]) + float(self.conf["field"]["y"])))**2 )
+        distancedif = distance_2 - distance_1
+        sonicspeed_1 = distancedif / timedif
+        overhead_1 = statistics.mean((self.calib_measurements["left"][1] - distance_1/sonicspeed_1, self.calib_measurements["left"][0] - distance_2/sonicspeed_1))
+
+        timedif = self.calib_measurements["right"][1] - self.calib_measurements["right"][0]
+        distance_1 = math.sqrt(float(self.ac_conf["right_x_offset"])**2 + (float(self.ac_conf["y_offset"]) + float(self.conf["field"]["y"]))**2 )
+        distance_2 = math.sqrt((float(self.ac_conf["right_x_offset"]) + float(self.conf["field"]["x"])**2 + (float(self.ac_conf["y_offset"]) + float(self.conf["field"]["y"])))**2 )
+        distancedif = distance_2 - distance_1
+        sonicspeed_2 = distancedif / timedif
+        overhead_2 = statistics.mean((self.calib_measurements["right"][0] - distance_1/sonicspeed_2, self.calib_measurements["right"][1] - distance_2/sonicspeed_2))
+        print(distance_1,sonicspeed_1,distance_2,sonicspeed_2)
+
+        self.ac_conf["sonicspeed"] = str(statistics.mean((sonicspeed_1,sonicspeed_2)))
+        self.ac_conf["overhead_left"]  = str(overhead_1)
+        self.ac_conf["overhead_right"] = str(overhead_2)
+        print("calibration result", float(self.ac_conf["sonicspeed"]), float(self.ac_conf["overhead_left"]), float(self.ac_conf["overhead_right"]))
+        self.calibration_state.next_state()
 
   def read(self):
     value = conn.getAcusticRTTs()
@@ -143,12 +117,12 @@ class AcusticSensor:
   def calculate_position(self,values):
     try:
       val1, val2 = values
-      val1 -= float(self.ac_conf["overhead"])
-      val2 -= float(self.ac_conf["overhead"])
-      distance_left = val1 * float(self.ac_conf["sonicspeed"]) # millisecond -> mikrosecond value of distance is in mm
+      val1 -= float(self.ac_conf["overhead_left"])
+      val2 -= float(self.ac_conf["overhead_right"])
+      distance_left = val1 * float(self.ac_conf["sonicspeed"])
       distance_right = val2 * float(self.ac_conf["sonicspeed"])
       x = (float(self.ac_conf["sensor_distance"])**2 - distance_right**2 + distance_left**2) / (2*float(self.ac_conf["sensor_distance"])) + float(self.ac_conf["left_x_offset"])
-      y = math.sqrt(abs(distance_left**2 - x**2)) + float(self.ac_conf["y_offset"])
+      y = math.sqrt(max(distance_left**2 - x**2, 0)) + float(self.ac_conf["y_offset"])
       return(x,y)
     except Exception as e:
       print(values)