changed sensor class

raspberry-pi/sensors.py

@@ -1,11 +1,15 @@
 from connection import ArduinoSlave
 import time
+import statistics
+import math
 
 conn = ArduinoSlave()
 
 class AcusticSensor:
   def __init__(self):
-    pass
+    self.sonic_speed_left = 0
+    self.sonic_speed_rigth = 0
+    self.sensor_distance = 450 #in mm
 
   def start(self):
     if not conn.isConnected():
@@ -15,11 +19,40 @@ class AcusticSensor:
   def _readCb(self, raw):
     print("acc: ", conn.getAcusticRTTs())
 
-  def calibrate(self, x, y):
-    pass
+  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)
 
   def read(self):
-    return (0, 0)
+    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)
 
 class MagneticSensor:
   def __init__(self):