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"] 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": calibration_thread = threading.Thread(target= self.calibrate) calibration_thread.start() elif action == "stop": print("exit Sensor") self.running = False thread.join() calibration_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.return_state() == 2: #first range of calibration values value = (1541+random.randint(-50,50),2076+random.randint(-50,50)) self.time_vals[0].append(value[0]) self.time_vals[1].append(value[1]) self.calibration_state.add_value() elif self.calibration_state.return_state() == 5: #second range of calibration values value = (2076+random.randint(-50,50),1541+random.randint(-50,50)) self.time_vals[0].append(value[0]) self.time_vals[1].append(value[1]) self.calibration_state.add_value() else: self.pass_to_gui(self.calculate_position(value)) time.sleep(0.01) def _readCb(self, raw): value = conn.getAcusticRTTs() print("acc: ", value) if self.calibration_state.return_state() == 2: #first range of calibration values self.time_vals[0].append(value[0]) self.time_vals[1].append(value[1]) self.calibration_state.add_value() elif self.calibration_state.return_state() == 5: #second range of calibration values self.time_vals[0].append(value[0]) self.time_vals[1].append(value[1]) self.calibration_state.add_value() else: self.pass_to_gui(self.calculate_position(value)) def calibrate(self): if not self.calibration_state.return_state() == 1: print("current calibration state:",self.calibration_state.return_state(),"need to be 1 to start calibration!") return else: print("start calibration") self.time_vals = [[],[]] self.calibration_state.next_state() while self.calibration_state.return_value_count() < 100: print("value count",self.calibration_state.return_value_count()) time.sleep(1) # wait until 100 values are gathered #todo add timeout if not self.running: self.calibration_state.reset_state() return left_time_1 = statistics.mean(self.time_vals[0]) right_time_2 = statistics.mean(self.time_vals[1]) self.calibration_state.next_state() # signal gui to get next position while self.calibration_state.return_state() != 4: time.sleep(1) # wait till ui is ready for second position #todo add timeout if not self.running: self.calibration_state.reset_state() return self.time_vals = [[],[]] self.calibration_state.reset_value_count() self.calibration_state.next_state() while self.calibration_state.return_value_count() < 100: print("value count",self.calibration_state.return_value_count()) time.sleep(1) # wait until 100 values are gathered #todo add timeout if not self.running: self.calibration_state.reset_state() return left_time_2 = statistics.mean(self.time_vals[0]) right_time_1 = statistics.mean(self.time_vals[1]) self.calibration_state.next_state() # signal gui start of calculation timedif = left_time_2 - left_time_1 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((left_time_1 - distance_1/sonicspeed_1,left_time_2 - distance_2/sonicspeed_1)) print(left_time_1,distance_1,sonicspeed_1,left_time_2,distance_2,sonicspeed_1) timedif = right_time_2 - right_time_1 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((right_time_1 - distance_1/sonicspeed_2,right_time_2 - distance_2/sonicspeed_2)) self.ac_conf["sonicspeed"] = str(statistics.mean((sonicspeed_1,sonicspeed_2))) self.ac_conf["overhead"] = str(statistics.mean((overhead_1,overhead_2))) print("calibration result",float(self.ac_conf["sonicspeed"]),float(self.ac_conf["overhead"])) 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"]) val2 -= float(self.ac_conf["overhead"]) distance_left = val1 * float(self.ac_conf["sonicspeed"]) # millisecond -> mikrosecond value of distance is in mm 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(abs(distance_left**2 - x**2)) + 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(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)