changed calibrating methode

raspberry-pi/main.py

@@ -1,21 +1,46 @@
 import sensors
 import time
+import threading
+import queue
 
-distance = 450 #in mm
+conf = {
+    "field":{
+        "x":450,
+        "y":450
+    },
+    "ac_sensor":{
+        "sensor_distance":450,
+        "y_offset":10,
+        "left_x_offset":0,
+        "rigth_x_offset":0,
+        "sonicspeed":343,
+        "overhead":50
+    }
+}
 
 def main():
-    ac_sensor = sensors.AcusticSensor()
-    ac_sensor.start()
-    ac_sensor.calibrate(distance)
+    up_queue = queue.Queue()
+    down_queue = queue.Queue()
+    calibration_sate = 0
+    ac_sensor = sensors.AcusticSensor(conf,up_queue,down_queue,calibration_sate)
+    sensor_thread = threading.Thread(target=ac_sensor.start)
+    sensor_thread.start()
+    time.sleep(3)
+    calibration_sate = 1
+    print(calibration_sate)
+    time.sleep(3)
+    down_queue.put("calibrate")
     try:
         while True:
-            x,y = ac_sensor.calculate_position()
-            print(x,y)
-            time.sleep(2)
+            time.sleep(1)
+            print(calibration_sate)
     except KeyboardInterrupt:
-        print("exit")
+        print("stop")
     except Exception as e:
         print("Error: ",e)
+    finally:
+        down_queue.put("stop")
+        sensor_thread.join()
 
 main()
 

raspberry-pi/sensors.py

@@ -2,57 +2,142 @@ from connection import ArduinoSlave
 import time
 import statistics
 import math
+import threading
+import noise
 
 conn = ArduinoSlave()
 
 class AcusticSensor:
-  def __init__(self):
-    self.sonic_speed_left = 0
-    self.sonic_speed_rigth = 0
-    self.sensor_distance = 450 #in mm
+  def __init__(self,conf,up_queue,down_queue,calibration_sate):
+    self.sensor_distance = conf["ac_sensor"]["sensor_distance"]
+    self.field_heigth = conf["field"]["y"]
+    self.field_length = conf["field"]["x"]
+    self.sensor_y_offset = conf["ac_sensor"]["y_offset"]
+    self.left_sensor_x_offset = conf["ac_sensor"]["left_x_offset"]
+    self.rigth_sensor_x_offset = conf["ac_sensor"]["rigth_x_offset"]
+    self.sonic_speed = conf["ac_sensor"]["sonicspeed"]
+    self.overhead = conf["ac_sensor"]["overhead"]
+    self.up_queue = up_queue
+    self.down_queue = down_queue
+    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()
+      if action == "calibrate":
+        self.calibrate()
+      elif action == "stop":
+        print("exit Sensor")
+        self.running = False
+        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
+        self.time_vals[0].append(value[0])
+        self.time_vals[1].append(value[1])
+        self.calibration_sate += 1
+
+      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
+
+      else:
+        self.pass_to_gui(self.calculate_position(value))
+      time.sleep(1)
+      self.n += 0.001
+
   def _readCb(self, raw):
-    print("acc: ", conn.getAcusticRTTs())
-
-  def calibrate(self, distance):
-    print("Move gondel to far left corner")
-    input()
-    speed = list()
-    for i in range(100):
-        time_val = [1.312,0]
-        speed.append(distance/time_val[0])
-    self.sonic_speed_left = statistics.mean(speed)
-    print("sonic speed left:",self.sonic_speed_left)
-
-    print("Move gondel to far rigth corner")
-    input()
-    speed = list()
-    for i in range(100):
-        time_val = [0,1.312]
-        speed.append(distance/time_val[1])
-    self.sonic_speed_rigth = statistics.mean(speed)
-    print("sonic speed rigth:",self.sonic_speed_rigth)
+    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
+
+    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
+
+    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!")
+      return
+    self.time_vals = [[],[]]
+    while len(self.time_vals) < 100:
+      print(len(self.time_vals))
+      time.sleep(1) # wait until 100 values are gathered
+      #todo add timeout
+    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
+
+    while self.calibration_sate != 103:
+      time.sleep(1) # wait till ui is ready for second position
+      #todo add timeout
+
+    self.time_vals = [[],[]]
+    while len(self.time_vals) < 100:
+      print(len(self.time_vals))
+      time.sleep(1) # wait until 100 values are gathered
+      #todo add timeout
+    
+    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
+
+    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 )
+    distance_2 = math.sqrt((self.left_sensor_x_offset + self.field_length)**2 + (self.sensor_y_offset + self.field_heigth)**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))
+
+    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 )
+    distance_2 = math.sqrt((self.rigth_sensor_x_offset + self.field_length)**2 + (self.sensor_y_offset + self.field_heigth)**2 )
+    distancedif = distance_2 - distance_1
+    sonicspeed_2 = distancedif / timedif
+    overhead_2 = statistics.mean((rigth_time_1 - distance_1/sonicspeed_2,rigth_time_2 - distance_2/sonicspeed_2))
+
+    self.sonic_speed = statistics.mean((sonicspeed_1,sonicspeed_2))
+    self.overhead = statistics.mean((overhead_1,overhead_2))
 
   def read(self):
-    return (952, 660)
-
-  def calculate_position(self):
-        if not self.sonic_speed_rigth or not self.sonic_speed_left:
-            print("sensor not calibrated! calibrate now!")
-            return (0,0)
-        else:
-            time_val = self.read()
-            distance_left = time_val[0] * self.sonic_speed_left / 1000 # seconds -> mseconds
-            distance_rigth = time_val[1] * self.sonic_speed_rigth / 1000
-            print(distance_left,distance_rigth)
-            x = (self.sensor_distance**2 - distance_rigth**2 + distance_left**2) / (2*self.sensor_distance)
-            y = math.sqrt(distance_left**2 - x**2)
-            return (x,y)
+    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)
+
+  def pass_to_gui(self,data):
+    self.up_queue.put(data)
 
 class MagneticSensor:
   def __init__(self):