acousticSensor.py 8.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188
  1. import time
  2. import statistics
  3. import math
  4. import threading
  5. import random
  6. import traceback
  7. import queue
  8. from sensors.calibration import CalibrationStateMashine
  9. from sensors.connection import globalArduinoSlave
  10. import logHandler
  11. conn = globalArduinoSlave()
  12. class AcousticSensor:
  13. def __init__(self, conf):
  14. self.conf = conf
  15. self.queue = queue.Queue()
  16. self.calibration_state = CalibrationStateMashine()
  17. self.field_height = float(conf["field"]["y"])
  18. self.field_width = float(conf["field"]["x"])
  19. self.sensor_y_offset = float(conf["ac_sensor"]["y_offset"])
  20. self.left_sensor_x_offset = float(conf["ac_sensor"]["left_x_offset"])
  21. self.right_sensor_x_offset = float(conf["ac_sensor"]["right_x_offset"])
  22. self.calibration_y_offset_1 = float(conf["ac_sensor"]["calibration_y_offset_1"])
  23. self.calibration_y_offset_2 = float(conf["ac_sensor"]["calibration_y_offset_2"])
  24. self.calibration_x_offset = float(conf["ac_sensor"]["calibration_x_offset"])
  25. self.sensor_distance = self.field_width - self.left_sensor_x_offset + self.right_sensor_x_offset
  26. self.sonic_speed = float(conf["ac_sensor"]["sonicspeed"])
  27. self.overhead_left = float(conf["ac_sensor"]["overhead_left"])
  28. self.overhead_right = float(conf["ac_sensor"]["overhead_right"])
  29. self.log_handler = logHandler.get_log_handler()
  30. # temporary calibration variables
  31. self.time_vals = [[],[]]
  32. self.cal_values = {
  33. "front": [0, 0],
  34. "back": [0, 0]
  35. }
  36. self.n = 0
  37. def start(self):
  38. self.log_handler.log_and_print("start acoustic sensor")
  39. if not conn.isConnected():
  40. conn.open(port = self.conf["arduino"]["port"])
  41. conn.addRecvCallback(self._readCb)
  42. # generate dummy values until arduino is ready
  43. self.dummyActive = True
  44. dummyThread = threading.Thread(target=self._readCb_dummy)
  45. dummyThread.start()
  46. def start_calibration(self):
  47. self.calibration_state.reset_state()
  48. self.time_vals = [[],[]]
  49. self.calibration_state.next_state()
  50. def stop(self):
  51. self.log_handler.log_and_print("stop acoustic sensor")
  52. self.dummyActive = False
  53. conn.close()
  54. def _readCb_dummy(self):
  55. self.log_handler.log_and_print("acoustic sensor: generating test values")
  56. while self.dummyActive:
  57. if self.n % 4 < 1:
  58. dummyPosition = (0, self.n%1 * self.field_height)
  59. elif self.n % 4 < 2:
  60. dummyPosition = (self.n%1 * self.field_width, self.field_height)
  61. elif self.n % 4 < 3:
  62. dummyPosition = (self.field_width, self.field_height - self.n%1 * self.field_height)
  63. else:
  64. dummyPosition = (self.field_width - self.n%1 * self.field_width, 0)
  65. self.n += 0.01
  66. if self.calibration_state.get_state() in [self.calibration_state.ACCUMULATING_1, self.calibration_state.WAITING_POS_1]:
  67. dummyPosition = (self.calibration_x_offset + random.randint(-5,5), self.calibration_y_offset_1 + random.randint(-5,5))
  68. elif self.calibration_state.get_state() in [self.calibration_state.ACCUMULATING_2, self.calibration_state.WAITING_POS_2]:
  69. dummyPosition = (self.calibration_x_offset + random.randint(-5,5), self.calibration_y_offset_2 + random.randint(-5,5))
  70. # these dummy parameters should also appear after the calibration
  71. dummyValue = self.predict_values(dummyPosition, sonic_speed=0.35, overhead_left=30, overhead_right=100)
  72. self.calibrate(dummyValue)
  73. position = self.calculate_position(dummyValue)
  74. if position != None:
  75. self.pass_to_gui(position + dummyValue)
  76. time.sleep(0.012)
  77. self.log_handler.log_and_print("acoustic sensor: disabled test mode")
  78. def _readCb(self, raw):
  79. if self.dummyActive == True:
  80. self.dummyActive = False
  81. value = conn.getAcousticRTTs()
  82. # partially missing values will be ignored
  83. if value[0] >= 0 and value[1] >= 0:
  84. self.calibrate(value)
  85. position = self.calculate_position(value)
  86. if position != None:
  87. self.pass_to_gui(position + value)
  88. def calibrate(self, value):
  89. if self.calibration_state.get_state() == self.calibration_state.ACCUMULATING_1:
  90. self.time_vals[0].append(value[0])
  91. self.time_vals[1].append(value[1])
  92. self.calibration_state.progress = len(self.time_vals[0]) / 2
  93. if len(self.time_vals[0]) >= 100:
  94. self.cal_values["front"][0] = statistics.mean(self.time_vals[0])
  95. self.cal_values["front"][1] = statistics.mean(self.time_vals[1])
  96. self.time_vals = [[],[]]
  97. self.calibration_state.next_state() # signal gui to get next position
  98. elif self.calibration_state.get_state() == self.calibration_state.ACCUMULATING_2:
  99. self.time_vals[0].append(value[0])
  100. self.time_vals[1].append(value[1])
  101. self.calibration_state.progress = 50 + len(self.time_vals[0]) / 2
  102. if len(self.time_vals[0]) >= 100:
  103. self.cal_values["back"][0] = statistics.mean(self.time_vals[0])
  104. self.cal_values["back"][1] = statistics.mean(self.time_vals[1])
  105. # all values have been captured
  106. self.log_handler.log_and_print("calibration measurements:", self.cal_values)
  107. # calculate distances from config
  108. # /|\
  109. # d1 d2 d3 / | \ d4
  110. # _..'|'.._y_off + calYoff_2 / | \y_off + calYoff_2
  111. # /____|____\ /___|___\
  112. # x_off x_off
  113. distance_1 = math.sqrt((self.calibration_x_offset + self.left_sensor_x_offset)**2 + (self.sensor_y_offset + self.calibration_y_offset_1)**2 )
  114. distance_2 = math.sqrt((self.calibration_x_offset + self.left_sensor_x_offset)**2 + (self.sensor_y_offset + self.calibration_y_offset_2)**2 )
  115. distancedif = distance_2 - distance_1
  116. timedif = self.cal_values["back"][0] - self.cal_values["front"][0]
  117. # speed of sound in mm/us
  118. sonicspeed_1 = distancedif / timedif
  119. # same for the second set of values
  120. distance_3 = math.sqrt((self.right_sensor_x_offset + (self.field_width - self.calibration_x_offset))**2 + (self.sensor_y_offset + self.calibration_y_offset_1)**2 )
  121. distance_4 = math.sqrt((self.right_sensor_x_offset + (self.field_width - self.calibration_x_offset))**2 + (self.sensor_y_offset + self.calibration_y_offset_2)**2 )
  122. distancedif = distance_4 - distance_3
  123. timedif = self.cal_values["back"][1] - self.cal_values["front"][1]
  124. sonicspeed_2 = distancedif / timedif
  125. # processing time overhead in us
  126. overhead_1 = statistics.mean((self.cal_values["front"][0] - distance_1/sonicspeed_1, self.cal_values["back"][0] - distance_2/sonicspeed_1))
  127. overhead_2 = statistics.mean((self.cal_values["front"][1] - distance_3/sonicspeed_2, self.cal_values["back"][1] - distance_4/sonicspeed_2))
  128. # calculate calibration results
  129. self.sonic_speed = statistics.mean((sonicspeed_1,sonicspeed_2))
  130. self.overhead_left = overhead_1
  131. self.overhead_right = overhead_2
  132. self.log_handler.log_and_print("calibration results:")
  133. self.log_handler.log_and_print(" sonicspeed: {:8.6f} mm/us".format(self.sonic_speed))
  134. self.log_handler.log_and_print(" overhead_left: {:8.3f} us".format(self.overhead_left))
  135. self.log_handler.log_and_print(" overhead_right: {:8.3f} us".format(self.overhead_right))
  136. self.calibrathttps://gogs.es-lab.de/wokoeck_ch/lern-tracking-systemion_state.next_state()
  137. def read(self):
  138. value = conn.getAcousticRTTs()
  139. return value
  140. def calculate_position(self, values):
  141. val1, val2 = values
  142. val1 -= self.overhead_left
  143. val2 -= self.overhead_right
  144. distance_left = val1 * self.sonic_speed
  145. distance_right = val2 * self.sonic_speed
  146. # compute intersection of distance circles
  147. x = (self.sensor_distance**2 - distance_right**2 + distance_left**2) / (2*self.sensor_distance) + self.left_sensor_x_offset
  148. if distance_left**2 - x**2 >= 0:
  149. y = math.sqrt(distance_left**2 - x**2) - self.sensor_y_offset
  150. return (x, y)
  151. else:
  152. return None
  153. def predict_values(self, position, sonic_speed, overhead_left, overhead_right):
  154. x, y = position
  155. distance_left = math.sqrt((y + self.sensor_y_offset)**2 + x**2)
  156. distance_right = math.sqrt(self.sensor_distance**2 + distance_left**2 - x * 2 * self.sensor_distance - x * self.left_sensor_x_offset)
  157. return (distance_left / sonic_speed + overhead_left, distance_right / sonic_speed + overhead_right)
  158. def pass_to_gui(self, data):
  159. self.queue.put(("data", data))