import time import statistics import math import threading import random import traceback from sensors.connection import globalArduinoSlave import logHandler conn = globalArduinoSlave() class AcusticSensor: def __init__(self, conf, ac_queue, calibration_state): self.conf = conf self.ac_queue = ac_queue self.calibration_state = calibration_state self.field_height = float(conf["field"]["y"]) self.field_width = float(conf["field"]["x"]) self.sensor_y_offset = float(conf["ac_sensor"]["y_offset"]) self.left_sensor_x_offset = float(conf["ac_sensor"]["left_x_offset"]) self.right_sensor_x_offset = float(conf["ac_sensor"]["right_x_offset"]) self.calibration_y_offset_1 = float(conf["ac_sensor"]["calibration_y_offset_1"]) self.calibration_y_offset_2 = float(conf["ac_sensor"]["calibration_y_offset_2"]) self.calibration_x_offset = float(conf["ac_sensor"]["calibration_x_offset"]) self.sensor_distance = self.field_width - self.left_sensor_x_offset + self.right_sensor_x_offset self.sonic_speed = float(conf["ac_sensor"]["sonicspeed"]) self.overhead_left = float(conf["ac_sensor"]["overhead_left"]) self.overhead_right = float(conf["ac_sensor"]["overhead_right"]) self.log_handler = logHandler.get_log_handler() self.log_handler.log_and_print("start acustic sensor") # temporary calibration variables self.time_vals = [[],[]] self.cal_values = { "front": [0, 0], "back": [0, 0] } self.n = 0 def start(self): if not conn.isConnected(): conn.open(port = self.conf["arduino"]["port"]) conn.addRecvCallback(self._readCb) # generate dummy values until arduino is ready self.dummyActive = True dummyThread = threading.Thread(target=self._readCb_dummy) dummyThread.start() def start_calibration(self): self.calibration_state.reset_state() self.time_vals = [[],[]] self.calibration_state.next_state() def stop(self): self.log_handler.log_and_print("stop acustic sensor") self.dummyActive = False conn.close() def _readCb_dummy(self): self.log_handler.log_and_print("acustic sensor: generating test values") while self.dummyActive: 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) self.n += 0.01 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)) # 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): if self.dummyActive == True: self.dummyActive = False value = conn.getAcusticRTTs() # partially missing values will be ignored if value[0] >= 0 and value[1] >= 0: self.calibrate(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: self.time_vals[0].append(value[0]) self.time_vals[1].append(value[1]) self.calibration_state.progress = len(self.time_vals[0]) / 2 if len(self.time_vals[0]) >= 100: self.cal_values["front"][0] = statistics.mean(self.time_vals[0]) self.cal_values["front"][1] = statistics.mean(self.time_vals[1]) self.time_vals = [[],[]] self.calibration_state.next_state() # signal gui to get next position elif self.calibration_state.get_state() == self.calibration_state.ACCUMULATING_2: self.time_vals[0].append(value[0]) self.time_vals[1].append(value[1]) self.calibration_state.progress = 50 + len(self.time_vals[0]) / 2 if len(self.time_vals[0]) >= 100: self.cal_values["back"][0] = statistics.mean(self.time_vals[0]) self.cal_values["back"][1] = statistics.mean(self.time_vals[1]) # all values have been captured self.log_handler.log_and_print("calibration measurements:", self.cal_values) # calculate distances from config # /|\ # d1 d2 d3 / | \ d4 # _..'|'.._y_off + calYoff_2 / | \y_off + calYoff_2 # /____|____\ /___|___\ # x_off x_off distance_1 = math.sqrt((self.calibration_x_offset + self.left_sensor_x_offset)**2 + (self.sensor_y_offset + self.calibration_y_offset_1)**2 ) distance_2 = math.sqrt((self.calibration_x_offset + self.left_sensor_x_offset)**2 + (self.sensor_y_offset + self.calibration_y_offset_2)**2 ) distancedif = distance_2 - distance_1 timedif = self.cal_values["back"][0] - self.cal_values["front"][0] # speed of sound in mm/us sonicspeed_1 = distancedif / timedif # same for the second set of values distance_3 = math.sqrt((self.right_sensor_x_offset + (self.field_width - self.calibration_x_offset))**2 + (self.sensor_y_offset + self.calibration_y_offset_1)**2 ) distance_4 = math.sqrt((self.right_sensor_x_offset + (self.field_width - self.calibration_x_offset))**2 + (self.sensor_y_offset + self.calibration_y_offset_2)**2 ) distancedif = distance_4 - distance_3 timedif = self.cal_values["back"][1] - self.cal_values["front"][1] sonicspeed_2 = distancedif / timedif # processing time overhead in us overhead_1 = statistics.mean((self.cal_values["front"][0] - distance_1/sonicspeed_1, self.cal_values["back"][0] - distance_2/sonicspeed_1)) overhead_2 = statistics.mean((self.cal_values["front"][1] - distance_3/sonicspeed_2, self.cal_values["back"][1] - distance_4/sonicspeed_2)) # calculate calibration results self.sonic_speed = statistics.mean((sonicspeed_1,sonicspeed_2)) self.overhead_left = overhead_1 self.overhead_right = overhead_2 self.log_handler.log_and_print("calibration results:") self.log_handler.log_and_print(" sonicspeed: {:8.6f} mm/us".format(self.sonic_speed)) self.log_handler.log_and_print(" overhead_left: {:8.3f} us".format(self.overhead_left)) self.log_handler.log_and_print(" overhead_right: {:8.3f} us".format(self.overhead_right)) self.calibration_state.next_state() def read(self): value = conn.getAcusticRTTs() return value 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))