combine main window and sensors in main function

raspberry-pi/gui/mainWindow.py

@@ -1,14 +1,18 @@
 import tkinter as tk
 import time
+import queue
 
-from graph import Graph
+import gui.graph as Graph
 
 class MainWindow(tk.Frame):
-  def __init__(self, root):
+  def __init__(self, root, up_queue, down_queue,calibration_state):
     self.root = root
+    self.calibration_state = calibration_state
+    self.down_queue = down_queue
+    self.up_queue = up_queue
     tk.Frame.__init__(self, root)
 
-    self.graph = Graph(self)
+    self.graph = Graph.Graph(self)
     self.graph.pack(fill=tk.BOTH, side=tk.LEFT, expand=True)
 
     self.controls = tk.Frame(self)
@@ -16,15 +20,37 @@ class MainWindow(tk.Frame):
 
     l = tk.Label(self.controls, text="your widgets go here...", anchor="c")
     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.pack(side="top")
+    cs = tk.Label(self.controls, text=str(self.calibration_state.return_state()), anchor="c")
+    cs.pack(side="top", fill="both", expand=True)
 
   def update(self):
     self.graph.update()
     self.root.after(30, self.update)
 
+  def calibrate(self):
+    self.calibration_state.reset_state()
+    self.calibration_state.next_state()
+    self.down_queue.put("calibrate")
+
+
+def start_mainwindow(up_queue,down_queue,calibration_sate):
+  root = tk.Tk()
+  root.title("Tracking System")
+  view = MainWindow(root,up_queue,down_queue,calibration_sate)
+  view.pack(side="top", fill="both", expand=True)
+  view.update()
+  root.mainloop()
+
 if __name__ == "__main__":
   root = tk.Tk()
+  up_queue = queue.Queue()
+  down_queue = queue.Queue()
   root.title("Tracking System")
-  view = MainWindow(root)
+  view = MainWindow(root,up_queue,down_queue,list())
   view.pack(side="top", fill="both", expand=True)
   view.update()
   root.mainloop()

raspberry-pi/main.py

@@ -2,6 +2,7 @@ import sensors
 import time
 import threading
 import queue
+import gui.mainWindow as mainWindow
 
 conf = {
     "field":{
@@ -13,34 +14,76 @@ conf = {
         "y_offset":10,
         "left_x_offset":0,
         "rigth_x_offset":0,
-        "sonicspeed":343,
-        "overhead":50
+        "sonicspeed":0.343,
+        "overhead":150
     }
 }
 
+class CalibrationStateMashine():
+
+    def __init__(self):
+        self.state = 0
+        self.value_count = 0
+
+    def state_clearname(self):
+        if self.state == 0:
+            return "not calibrated"
+        elif self.state == 1:
+            return "ready on position one"
+        elif self.state == 2:
+            return "gathering values on position one"
+        elif self.state == 3:
+            return "finished gathering values from position one"
+        elif self.state == 4:
+            return "ready on position two"
+        elif self.state == 5:
+            return "gathering values on position two"
+        elif self.state == 6:
+            return "calculating calibration values"
+        elif self.state == 7:
+            return "calibration done"
+    
+    def next_state(self):
+        if self.state < 7:
+            self.state += 1
+            print(self.state_clearname())
+
+    def add_value(self):
+        self.value_count += 1
+    
+    def return_state(self):
+        return self.state
+
+    def return_value_count(self):
+        return self.value_count
+
+    def reset_state(self):
+        self.state = 0
+    
+    def reset_value_count(self):
+        self.value_count = 0
+
+
 def main():
     up_queue = queue.Queue()
     down_queue = queue.Queue()
-    calibration_sate = 0
+    calibration_sate = CalibrationStateMashine()
     ac_sensor = sensors.AcusticSensor(conf,up_queue,down_queue,calibration_sate)
     sensor_thread = threading.Thread(target=ac_sensor.start)
+
+    ui_thread = threading.Thread(target=mainWindow.start_mainwindow, args=(up_queue,down_queue,calibration_sate))
+
     sensor_thread.start()
-    time.sleep(3)
-    calibration_sate = 1
-    print(calibration_sate)
-    time.sleep(3)
-    down_queue.put("calibrate")
+    ui_thread.start()
     try:
         while True:
             time.sleep(1)
-            print(calibration_sate)
     except KeyboardInterrupt:
         print("stop")
     except Exception as e:
         print("Error: ",e)
     finally:
         down_queue.put("stop")
-        sensor_thread.join()
 
 main()
 

raspberry-pi/sensors.py

@@ -4,6 +4,8 @@ import statistics
 import math
 import threading
 import noise
+import random
+import traceback
 
 conn = ArduinoSlave()
 
@@ -22,87 +24,101 @@ class AcusticSensor:
     self.calibration_sate = calibration_sate
 
   def start(self):
-    self.n = 0.001
     self.running = True
     if not conn.isConnected():
       conn.open()
     conn.addRecvCallback(self._readCb)
-
     thread = threading.Thread(target=self._readCb_dummy)
     thread.start()
-
     while True:
       action = self.down_queue.get()
+      print("action",action)
       if action == "calibrate":
-        self.calibrate()
+        calibration_thread = threading.Thread(target= self.calibrate)
+        calibration_thread.start()
       elif action == "stop":
         print("exit Sensor")
         self.running = False
         thread.join()
+        calibration_thread.join()
         break
     conn.close()
 
   def _readCb_dummy(self):
     while self.running:
-      value = (980,660)
-      print("dummy acc: ", value)
-      if self.calibration_sate >=1 and self.calibration_sate < 101: #first range of calibration values
+      value = (900+random.randint(0,300),900+random.randint(0,300))
+      #print("dummy acc: ", value)
+      if self.calibration_sate.return_state() == 2: #first range of calibration values
+        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_sate += 1
+        self.calibration_sate.add_value()
 
-      elif self.calibration_sate >=103 and self.calibration_sate < 203: #second range of calibration values
+      elif self.calibration_sate.return_state() == 5: #second range of calibration values
+        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_sate += 1
+        self.calibration_sate.add_value()
 
       else:
         self.pass_to_gui(self.calculate_position(value))
-      time.sleep(1)
-      self.n += 0.001
+      time.sleep(0.01)
 
   def _readCb(self, raw):
     value = conn.getAcusticRTTs()
     print("acc: ", value)
-    if self.calibration_sate >=1 and self.calibration_sate < 101: #first range of calibration values
-      self.time_vals[0].append(value[0])
-      self.time_vals[1].append(value[1])
-      self.calibration_sate += 1
+    if self.calibration_sate.return_state() == 2: #first range of calibration values
+        self.time_vals[0].append(value[0])
+        self.time_vals[1].append(value[1])
+        self.calibration_sate.add_value()
 
-    elif self.calibration_sate >=103 and self.calibration_sate < 203: #second range of calibration values
-      self.time_vals[0].append(value[0])
-      self.time_vals[1].append(value[1])
-      self.calibration_sate += 1
+    elif self.calibration_sate.return_state() == 5: #second range of calibration values
+        self.time_vals[0].append(value[0])
+        self.time_vals[1].append(value[1])
+        self.calibration_sate.add_value()
 
     else:
       self.pass_to_gui(self.calculate_position(value))
 
   def calibrate(self):
-    if not self.calibration_sate == 1:
-      print("current calibration state:",self.calibration_sate,"need to be 1 to start calibration!")
+    if not self.calibration_sate.return_state() == 1:
+      print("current calibration state:",self.calibration_sate.return_state(),"need to be 1 to start calibration!")
       return
+    else:
+      print("start calibration")
     self.time_vals = [[],[]]
-    while len(self.time_vals) < 100:
-      print(len(self.time_vals))
+    self.calibration_sate.next_state()
+    while self.calibration_sate.return_value_count() < 100:
+      print("value count",self.calibration_sate.return_value_count())
       time.sleep(1) # wait until 100 values are gathered
       #todo add timeout
+      if not self.running:
+        self.calibration_sate.reset_state()
+        return
     left_time_1 = statistics.mean(self.time_vals[0])
     rigth_time_2 = statistics.mean(self.time_vals[1])
-    self.calibration_sate += 1 # signal gui to get next position
+    self.calibration_sate.next_state() # signal gui to get next position
 
-    while self.calibration_sate != 103:
+    while self.calibration_sate.return_state() != 4:
       time.sleep(1) # wait till ui is ready for second position
       #todo add timeout
+      if not self.running:
+        self.calibration_sate.reset_state()
+        return
 
     self.time_vals = [[],[]]
-    while len(self.time_vals) < 100:
-      print(len(self.time_vals))
+    self.calibration_sate.reset_value_count()
+    self.calibration_sate.next_state()
+    while self.calibration_sate.return_value_count() < 100:
+      print("value count",self.calibration_sate.return_value_count())
       time.sleep(1) # wait until 100 values are gathered
       #todo add timeout
-    
+      if not self.running:
+        self.calibration_sate.reset_state()
+        return
     left_time_2 = statistics.mean(self.time_vals[0])
     rigth_time_1 = statistics.mean(self.time_vals[1])
-    self.calibration_sate += 1 # signal gui start of calculation
+    self.calibration_sate.next_state() # signal gui start of calculation
 
     timedif = left_time_2 - left_time_1
     distance_1 = math.sqrt(self.left_sensor_x_offset**2 + (self.sensor_y_offset + self.field_heigth)**2 )
@@ -110,6 +126,7 @@ class AcusticSensor:
     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 = rigth_time_2 - rigth_time_1
     distance_1 = math.sqrt(self.rigth_sensor_x_offset**2 + (self.sensor_y_offset + self.field_heigth)**2 )
@@ -120,21 +137,26 @@ class AcusticSensor:
 
     self.sonic_speed = statistics.mean((sonicspeed_1,sonicspeed_2))
     self.overhead = statistics.mean((overhead_1,overhead_2))
+    print("calibration result",self.sonic_speed,self.overhead)
+    self.calibration_sate.next_state()
 
   def read(self):
     value = conn.getAcusticRTTs()
     return value
 
   def calculate_position(self,values):
-    val1, val2 = values
-    val1 -= self.overhead
-    val2 -= self.overhead
-    distance_left = val1 * self.sonic_speed / 1000 # millisecond -> mikrosecond value of distance is in mm
-    distance_rigth = val2 * self.sonic_speed / 1000
-    print(distance_left,distance_rigth)
-    x = (self.sensor_distance**2 - distance_rigth**2 + distance_left**2) / (2*self.sensor_distance) + self.left_sensor_x_offset
-    y = math.sqrt(distance_left**2 - x**2) + self.sensor_y_offset
-    return(x,y)
+    try:
+      val1, val2 = values
+      val1 -= self.overhead
+      val2 -= self.overhead
+      distance_left = val1 * self.sonic_speed # millisecond -> mikrosecond value of distance is in mm
+      distance_rigth = val2 * self.sonic_speed
+      x = (self.sensor_distance**2 - distance_rigth**2 + distance_left**2) / (2*self.sensor_distance) + self.left_sensor_x_offset
+      y = math.sqrt(distance_left**2 - x**2) + self.sensor_y_offset
+      return(x,y)
+    except Exception as e:
+      print(values)
+      traceback.print_exc()
 
   def pass_to_gui(self,data):
     self.up_queue.put(data)

raspberry-pi/test.py

@@ -0,0 +1,4 @@
+import noise
+
+for i in range(100000):
+    print(noise.pnoise1(i/1000)*1000)