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.sensor_distance = conf["ac_sensor"]["sensor_distance"]
self.field_heigth = conf["field"]["y"]
self.field_length = conf["field"]["x"]
self.sensor_y_offset = conf["ac_sensor"]["y_offset"]
self.left_sensor_x_offset = conf["ac_sensor"]["left_x_offset"]
self.rigth_sensor_x_offset = conf["ac_sensor"]["rigth_x_offset"]
self.sonic_speed = conf["ac_sensor"]["sonicspeed"]
self.overhead = conf["ac_sensor"]["overhead"]
self.up_queue = up_queue
self.down_queue = down_queue
self.calibration_state = calibration_state
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])
rigth_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])
rigth_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(self.left_sensor_x_offset**2 + (self.sensor_y_offset + self.field_heigth)**2 )
distance_2 = math.sqrt((self.left_sensor_x_offset + self.field_length)**2 + (self.sensor_y_offset + self.field_heigth)**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 = rigth_time_2 - rigth_time_1
distance_1 = math.sqrt(self.rigth_sensor_x_offset**2 + (self.sensor_y_offset + self.field_heigth)**2 )
distance_2 = math.sqrt((self.rigth_sensor_x_offset + self.field_length)**2 + (self.sensor_y_offset + self.field_heigth)**2 )
distancedif = distance_2 - distance_1
sonicspeed_2 = distancedif / timedif
overhead_2 = statistics.mean((rigth_time_1 - distance_1/sonicspeed_2,rigth_time_2 - distance_2/sonicspeed_2))
self.sonic_speed = statistics.mean((sonicspeed_1,sonicspeed_2))
self.overhead = statistics.mean((overhead_1,overhead_2))
print("calibration result",self.sonic_speed,self.overhead)
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
val2 -= self.overhead
distance_left = val1 * self.sonic_speed # millisecond -> mikrosecond value of distance is in mm
distance_rigth = val2 * self.sonic_speed
x = (self.sensor_distance**2 - distance_rigth**2 + distance_left**2) / (2*self.sensor_distance) + self.left_sensor_x_offset
y = math.sqrt(distance_left**2 - x**2) + 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(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)