|
@@ -12,7 +12,7 @@ import cv2
|
|
|
conn = ArduinoSlave()
|
|
|
|
|
|
class AcusticSensor:
|
|
|
- def __init__(self,conf,up_queue,down_queue,calibration_sate):
|
|
|
+ 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"]
|
|
@@ -23,7 +23,7 @@ class AcusticSensor:
|
|
|
self.overhead = conf["ac_sensor"]["overhead"]
|
|
|
self.up_queue = up_queue
|
|
|
self.down_queue = down_queue
|
|
|
- self.calibration_sate = calibration_sate
|
|
|
+ self.calibration_state = calibration_state
|
|
|
|
|
|
def start(self):
|
|
|
self.running = True
|
|
@@ -50,17 +50,17 @@ class AcusticSensor:
|
|
|
while self.running:
|
|
|
value = (900+random.randint(0,300),900+random.randint(0,300))
|
|
|
|
|
|
- if self.calibration_sate.return_state() == 2:
|
|
|
+ if self.calibration_state.return_state() == 2:
|
|
|
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_sate.add_value()
|
|
|
+ self.calibration_state.add_value()
|
|
|
|
|
|
- elif self.calibration_sate.return_state() == 5:
|
|
|
+ elif self.calibration_state.return_state() == 5:
|
|
|
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_sate.add_value()
|
|
|
+ self.calibration_state.add_value()
|
|
|
|
|
|
else:
|
|
|
self.pass_to_gui(self.calculate_position(value))
|
|
@@ -69,58 +69,58 @@ class AcusticSensor:
|
|
|
def _readCb(self, raw):
|
|
|
value = conn.getAcusticRTTs()
|
|
|
print("acc: ", value)
|
|
|
- if self.calibration_sate.return_state() == 2:
|
|
|
+ if self.calibration_state.return_state() == 2:
|
|
|
self.time_vals[0].append(value[0])
|
|
|
self.time_vals[1].append(value[1])
|
|
|
- self.calibration_sate.add_value()
|
|
|
+ self.calibration_state.add_value()
|
|
|
|
|
|
- elif self.calibration_sate.return_state() == 5:
|
|
|
+ elif self.calibration_state.return_state() == 5:
|
|
|
self.time_vals[0].append(value[0])
|
|
|
self.time_vals[1].append(value[1])
|
|
|
- self.calibration_sate.add_value()
|
|
|
+ self.calibration_state.add_value()
|
|
|
|
|
|
else:
|
|
|
self.pass_to_gui(self.calculate_position(value))
|
|
|
|
|
|
def calibrate(self):
|
|
|
- if not self.calibration_sate.return_state() == 1:
|
|
|
- print("current calibration state:",self.calibration_sate.return_state(),"need to be 1 to start calibration!")
|
|
|
+ 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_sate.next_state()
|
|
|
- while self.calibration_sate.return_value_count() < 100:
|
|
|
- print("value count",self.calibration_sate.return_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)
|
|
|
|
|
|
if not self.running:
|
|
|
- self.calibration_sate.reset_state()
|
|
|
+ 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_sate.next_state()
|
|
|
+ self.calibration_state.next_state()
|
|
|
|
|
|
- while self.calibration_sate.return_state() != 4:
|
|
|
+ while self.calibration_state.return_state() != 4:
|
|
|
time.sleep(1)
|
|
|
|
|
|
if not self.running:
|
|
|
- self.calibration_sate.reset_state()
|
|
|
+ self.calibration_state.reset_state()
|
|
|
return
|
|
|
|
|
|
self.time_vals = [[],[]]
|
|
|
- self.calibration_sate.reset_value_count()
|
|
|
- self.calibration_sate.next_state()
|
|
|
- while self.calibration_sate.return_value_count() < 100:
|
|
|
- print("value count",self.calibration_sate.return_value_count())
|
|
|
+ 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)
|
|
|
|
|
|
if not self.running:
|
|
|
- self.calibration_sate.reset_state()
|
|
|
+ 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_sate.next_state()
|
|
|
+ self.calibration_state.next_state()
|
|
|
|
|
|
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 )
|
|
@@ -140,7 +140,7 @@ class AcusticSensor:
|
|
|
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_sate.next_state()
|
|
|
+ self.calibration_state.next_state()
|
|
|
|
|
|
def read(self):
|
|
|
value = conn.getAcusticRTTs()
|