from connection import ArduinoSlave import markerDetection import time import statistics import math import threading import noise import random import traceback import cv2 conn = ArduinoSlave() class AcusticSensor: def __init__(self,conf,up_queue,down_queue,calibration_state): self.up_queue = up_queue self.down_queue = down_queue self.calibration_state = calibration_state self.conf = conf self.ac_conf = conf["ac_sensor"] self.time_vals = [[],[]] self.calib_measurements = { "left": [0, 0], "right": [0, 0] } def start(self): 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() print("action",action) if action == "calibrate": self.calibration_state.reset_state() self.time_vals = [[],[]] self.calibration_state.next_state() elif action == "stop": print("exit Sensor") self.running = False thread.join() break conn.close() def _readCb_dummy(self): while self.running: value = (900+random.randint(0,300),900+random.randint(0,300)) #print("dummy acc: ", value) if self.calibration_state.get_state() == self.calibration_state.ACCUMULATING_1: value = (1541+random.randint(-50,50),2076+random.randint(-50,50)) elif self.calibration_state.get_state() == self.calibration_state.ACCUMULATING_2: value = (2076+random.randint(-50,50),1541+random.randint(-50,50)) self.calibrate(value) self.pass_to_gui(self.calculate_position(value)) time.sleep(0.01) def _readCb(self, raw): value = conn.getAcusticRTTs() print("acc: ", value) self.calibrate(value) self.pass_to_gui(self.calculate_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]) if len(self.time_vals[0]) >= 100: self.calib_measurements["left"][0] = statistics.mean(self.time_vals[0]) self.calib_measurements["right"][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]) if len(self.time_vals[0]) >= 100: self.calib_measurements["left"][1] = statistics.mean(self.time_vals[0]) self.calib_measurements["right"][0] = statistics.mean(self.time_vals[1]) # all values have been captured print(self.calib_measurements) # calculate calibration results timedif = self.calib_measurements["left"][1] - self.calib_measurements["left"][0] distance_1 = math.sqrt(float(self.ac_conf["left_x_offset"])**2 + (float(self.ac_conf["y_offset"]) + float(self.conf["field"]["y"]))**2 ) distance_2 = math.sqrt((float(self.ac_conf["left_x_offset"]) + float(self.conf["field"]["x"])**2 + (float(self.ac_conf["y_offset"]) + float(self.conf["field"]["y"])))**2 ) distancedif = distance_2 - distance_1 sonicspeed_1 = distancedif / timedif overhead_1 = statistics.mean((self.calib_measurements["left"][1] - distance_1/sonicspeed_1, self.calib_measurements["left"][0] - distance_2/sonicspeed_1)) timedif = self.calib_measurements["right"][1] - self.calib_measurements["right"][0] distance_1 = math.sqrt(float(self.ac_conf["right_x_offset"])**2 + (float(self.ac_conf["y_offset"]) + float(self.conf["field"]["y"]))**2 ) distance_2 = math.sqrt((float(self.ac_conf["right_x_offset"]) + float(self.conf["field"]["x"])**2 + (float(self.ac_conf["y_offset"]) + float(self.conf["field"]["y"])))**2 ) distancedif = distance_2 - distance_1 sonicspeed_2 = distancedif / timedif overhead_2 = statistics.mean((self.calib_measurements["right"][0] - distance_1/sonicspeed_2, self.calib_measurements["right"][1] - distance_2/sonicspeed_2)) print(distance_1,sonicspeed_1,distance_2,sonicspeed_2) self.ac_conf["sonicspeed"] = str(statistics.mean((sonicspeed_1,sonicspeed_2))) self.ac_conf["overhead_left"] = str(overhead_1) self.ac_conf["overhead_right"] = str(overhead_2) print("calibration result", float(self.ac_conf["sonicspeed"]), float(self.ac_conf["overhead_left"]), float(self.ac_conf["overhead_right"])) self.calibration_state.next_state() def read(self): value = conn.getAcusticRTTs() return value def calculate_position(self,values): try: val1, val2 = values val1 -= float(self.ac_conf["overhead_left"]) val2 -= float(self.ac_conf["overhead_right"]) distance_left = val1 * float(self.ac_conf["sonicspeed"]) distance_right = val2 * float(self.ac_conf["sonicspeed"]) x = (float(self.ac_conf["sensor_distance"])**2 - distance_right**2 + distance_left**2) / (2*float(self.ac_conf["sensor_distance"])) + float(self.ac_conf["left_x_offset"]) y = math.sqrt(max(distance_left**2 - x**2, 0)) + float(self.ac_conf["y_offset"]) return(x,y) except Exception as e: print(values) traceback.print_exc() def pass_to_gui(self,data): self.up_queue.put(("ac_data", data)) class MagneticSensor: def __init__(self): pass def start(self): if not conn.isConnected(): conn.open() conn.addRecvCallback(self._readCb) def _readCb(self, raw): print("mag: ", conn.getMagneticField()) def calibrate(self, x, y): pass def read(self): return conn.getMagneticField() class OpticalSensor(): def __init__(self): self.cap = cv2.VideoCapture(0) self._t = None self.values = None def start(self): if not self._t: self._t = threading.Thread(target=self._getFrames, args=()) self._t.daemon = True # thread dies when main thread (only non-daemon thread) exits. self._t.start() def _getFrames(self): while True: success, image = self.cap.read() if success: self.values = markerDetection.measureDistances(image) print("opt:", self.values) def calibrate(self, x, y): pass def read(self): return self.values if __name__ == "__main__": acc = AcusticSensor() acc.start() mag = MagneticSensor() mag.start() opt = OpticalSensor() opt.start() while True: time.sleep(1)