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.field_height = float(conf["field"]["y"]) self.field_length = float(conf["field"]["x"]) self.sensor_distance = float(conf["ac_sensor"]["sensor_distance"]) 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.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.time_vals = [[],[]] self.cal_values = { "left": [0, 0], "right": [0, 0] } self.n = 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)) value = ((noise.pnoise1(self.n)+1)*150+900, (noise.pnoise1(self.n*1.3+3)+1)*150+900) self.n += 0.01 #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.cal_values["left"][0] = statistics.mean(self.time_vals[0]) self.cal_values["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.cal_values["left"][1] = statistics.mean(self.time_vals[0]) self.cal_values["right"][0] = statistics.mean(self.time_vals[1]) # all values have been captured print(self.cal_values) # calculate calibration results timedif = self.cal_values["left"][1] - self.cal_values["left"][0] distance_1 = math.sqrt(self.left_sensor_x_offset**2 + (self.sensor_y_offset + self.field_height)**2 ) distance_2 = math.sqrt((self.left_sensor_x_offset + self.field_length)**2 + (self.sensor_y_offset + self.field_height)**2 ) distancedif = distance_2 - distance_1 sonicspeed_1 = distancedif / timedif overhead_1 = statistics.mean((self.cal_values["left"][1] - distance_1/sonicspeed_1, self.cal_values["left"][0] - distance_2/sonicspeed_1)) timedif = self.cal_values["right"][1] - self.cal_values["right"][0] distance_1 = math.sqrt(self.right_sensor_x_offset**2 + (self.sensor_y_offset + self.field_height)**2 ) distance_2 = math.sqrt((self.right_sensor_x_offset + self.field_length)**2 + (self.sensor_y_offset + self.field_height)**2 ) distancedif = distance_2 - distance_1 sonicspeed_2 = distancedif / timedif overhead_2 = statistics.mean((self.cal_values["right"][0] - distance_1/sonicspeed_2, self.cal_values["right"][1] - distance_2/sonicspeed_2)) print(distance_1,sonicspeed_1,distance_2,sonicspeed_2) self.sonic_speed = statistics.mean((sonicspeed_1,sonicspeed_2)) self.overhead_left = overhead_1 self.overhead_right = overhead_2 print("calibration result", self.sonic_speed, self.overhead_left, self.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 -= self.overhead_left val2 -= self.overhead_right distance_left = val1 * self.sonic_speed distance_right = val2 * self.sonic_speed 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) 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)