improved dummy calibration

raspberry-pi/sensors/acusticSensor.py

@@ -62,19 +62,31 @@ class AcusticSensor:
   def _readCb_dummy(self):
     self.log_handler.log_and_print("acustic sensor: generating test values")
     while self.dummyActive:
-      value = (900+random.randint(0,300),900+random.randint(0,300))
-      value = ((math.sin(self.n)+1)*400+900, (math.cos(self.n)+1)*400+900)
-      self.n += 0.02
+      if self.n % 4 < 1:
+        dummyPosition = (0, self.n%1 * self.field_height)
+      elif self.n % 4 < 2:
+        dummyPosition = (self.n%1 * self.field_width, self.field_height)
+      elif self.n % 4 < 3:
+        dummyPosition = (self.field_width, self.field_height - self.n%1 * self.field_height)
+      else:
+        dummyPosition = (self.field_width - self.n%1 * self.field_width, 0)
 
-      if self.calibration_state.get_state() == self.calibration_state.ACCUMULATING_1:
-        value = (750+random.randint(-50,50),750+random.randint(-50,50))
+      self.n += 0.01
 
-      elif self.calibration_state.get_state() == self.calibration_state.ACCUMULATING_2:
-        value = (1450+random.randint(-50,50),1450+random.randint(-50,50))
+      if self.calibration_state.get_state() in [self.calibration_state.ACCUMULATING_1, self.calibration_state.WAITING_POS_1]:
+        dummyPosition = (self.calibration_x_offset + random.randint(-5,5), self.calibration_y_offset_1 + random.randint(-5,5))
+        
+      elif self.calibration_state.get_state() in [self.calibration_state.ACCUMULATING_2, self.calibration_state.WAITING_POS_2]:
+        dummyPosition = (self.calibration_x_offset + random.randint(-5,5), self.calibration_y_offset_2 + random.randint(-5,5))
 
-      self.calibrate(value)
-      self.pass_to_gui(self.calculate_position(value) + value)
-      time.sleep(0.01)
+      # these dummy parameters should also appear after the calibration
+      dummyValue = self.predict_values(dummyPosition, sonic_speed=0.35, overhead_left=30, overhead_right=100)
+
+      self.calibrate(dummyValue)
+      position = self.calculate_position(dummyValue)
+      if position != None:
+        self.pass_to_gui(position + dummyValue)
+      time.sleep(0.012)
     self.log_handler.log_and_print("acustic sensor: disabled test mode")
 
   def _readCb(self, raw):
@@ -150,23 +162,25 @@ class AcusticSensor:
     value = conn.getAcusticRTTs()
     return value
 
-  def calculate_position(self,values):
-    try:
-      val1, val2 = values
-      val1 -= self.overhead_left
-      val2 -= self.overhead_right
-      distance_left = val1 * self.sonic_speed
-      distance_right = val2 * self.sonic_speed
-      # compute intersection of distance circles
-      x = (self.sensor_distance**2 - distance_right**2 + distance_left**2) / (2*self.sensor_distance) + self.left_sensor_x_offset
-      if distance_left**2 - x**2 >= 0:
-        y = math.sqrt(distance_left**2 - x**2) - self.sensor_y_offset
-        return (x, y)
-      else:
-        return None
-    except Exception as e:
-      print(values)
-      traceback.print_exc()
+  def calculate_position(self, values):
+    val1, val2 = values
+    val1 -= self.overhead_left
+    val2 -= self.overhead_right
+    distance_left = val1 * self.sonic_speed
+    distance_right = val2 * self.sonic_speed
+    # compute intersection of distance circles
+    x = (self.sensor_distance**2 - distance_right**2 + distance_left**2) / (2*self.sensor_distance) + self.left_sensor_x_offset
+    if distance_left**2 - x**2 >= 0:
+      y = math.sqrt(distance_left**2 - x**2) - self.sensor_y_offset
+      return (x, y)
+    else:
+      return None
+
+  def predict_values(self, position, sonic_speed, overhead_left, overhead_right):
+    x, y = position
+    distance_left = math.sqrt((y + self.sensor_y_offset)**2 + x**2)
+    distance_right = math.sqrt(self.sensor_distance**2 + distance_left**2 - x * 2 * self.sensor_distance - x * self.left_sensor_x_offset)
+    return (distance_left / sonic_speed + overhead_left, distance_right / sonic_speed + overhead_right)
 
   def pass_to_gui(self, data):
     self.ac_queue.put(("data", data))