|
|
@@ -74,7 +74,9 @@ class AcusticSensor:
|
|
|
# partially missing values will be ignored
|
|
|
if value[0] >= 0 and value[1] >= 0:
|
|
|
self.calibrate(value)
|
|
|
- self.pass_to_gui(self.calculate_position(value) + value)
|
|
|
+ position = self.calculate_position(value)
|
|
|
+ if position != None:
|
|
|
+ self.pass_to_gui(position + value)
|
|
|
|
|
|
def calibrate(self, value):
|
|
|
if self.calibration_state.get_state() == self.calibration_state.ACCUMULATING_1:
|
|
|
@@ -144,11 +146,14 @@ class AcusticSensor:
|
|
|
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
|
|
|
- y = math.sqrt(max(distance_left**2 - x**2, 0)) - self.sensor_y_offset
|
|
|
- return (x, y)
|
|
|
+ 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 pass_to_gui(self, data):
|
|
|
- self.ac_queue.put(("data", data))
|
|
|
+ self.ac_queue.put(("data", data))
|
subDesTagesMitExtraKaese