lern-tracking-system/raspberry-pi/sensors/acusticSensor.py

from sensors.connection import globalArduinoSlave
import time
import statistics
import math
import threading
import random
import traceback

conn = globalArduinoSlave()

class AcusticSensor:
  def __init__(self, conf, ac_queue, calibration_state):
    self.ac_queue               = ac_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.dummyActive = True
    if not conn.isConnected():
      conn.open()
    conn.addRecvCallback(self._readCb)
    dummyThread = threading.Thread(target=self._readCb_dummy)
    dummyThread.start()
    
  def start_calibration(self):
    self.calibration_state.reset_state()
    self.time_vals = [[],[]]
    self.calibration_state.next_state()

  def stop(self):
    print("stop acoustic sensor")
    self.dummyActive = False
    conn.close()

  def _readCb_dummy(self):
    while self.dummyActive:
      value = (900+random.randint(0,300),900+random.randint(0,300))
      value = ((math.sin(self.n)+1)*400+900, (math.cos(self.n)+1)*400+900)
      self.n += 0.02

      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()

    if self.dummyActive:
      self.dummyActive = False

    if value[0] >= 0 and value[1] >= 0:
      self.calibrate(value)
      x, y = self.calculate_position(value)
      self.pass_to_gui((x, y, *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))

        self.sonic_speed = statistics.mean((sonicspeed_1,sonicspeed_2))
        self.overhead_left  = overhead_1
        self.overhead_right = overhead_2
        print("calibration results:")
        print("  sonicspeed:     {:8.6f} mm/us".format(self.sonic_speed))
        print("  overhead_left:  {:8.3f} us".format(self.overhead_left))
        print("  overhead_right: {:8.3f} us".format(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.ac_queue.put(("data", data))