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

from connection import ArduinoSlave
import time
import statistics
import math
import threading
import noise

conn = ArduinoSlave()

class AcusticSensor:
  def __init__(self,conf,up_queue,down_queue,calibration_sate):
    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_sate = calibration_sate

  def start(self):
    self.n = 0.001
    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()
      if action == "calibrate":
        self.calibrate()
      elif action == "stop":
        print("exit Sensor")
        self.running = False
        thread.join()
        break
    conn.close()

  def _readCb_dummy(self):
    while self.running:
      value = (980,660)
      print("dummy acc: ", value)
      if self.calibration_sate >=1 and self.calibration_sate < 101: #first range of calibration values
        self.time_vals[0].append(value[0])
        self.time_vals[1].append(value[1])
        self.calibration_sate += 1

      elif self.calibration_sate >=103 and self.calibration_sate < 203: #second range of calibration values
        self.time_vals[0].append(value[0])
        self.time_vals[1].append(value[1])
        self.calibration_sate += 1

      else:
        self.pass_to_gui(self.calculate_position(value))
      time.sleep(1)
      self.n += 0.001

  def _readCb(self, raw):
    value = conn.getAcusticRTTs()
    print("acc: ", value)
    if self.calibration_sate >=1 and self.calibration_sate < 101: #first range of calibration values
      self.time_vals[0].append(value[0])
      self.time_vals[1].append(value[1])
      self.calibration_sate += 1

    elif self.calibration_sate >=103 and self.calibration_sate < 203: #second range of calibration values
      self.time_vals[0].append(value[0])
      self.time_vals[1].append(value[1])
      self.calibration_sate += 1

    else:
      self.pass_to_gui(self.calculate_position(value))

  def calibrate(self):
    if not self.calibration_sate == 1:
      print("current calibration state:",self.calibration_sate,"need to be 1 to start calibration!")
      return
    self.time_vals = [[],[]]
    while len(self.time_vals) < 100:
      print(len(self.time_vals))
      time.sleep(1) # wait until 100 values are gathered
      #todo add timeout
    left_time_1 = statistics.mean(self.time_vals[0])
    rigth_time_2 = statistics.mean(self.time_vals[1])
    self.calibration_sate += 1 # signal gui to get next position

    while self.calibration_sate != 103:
      time.sleep(1) # wait till ui is ready for second position
      #todo add timeout

    self.time_vals = [[],[]]
    while len(self.time_vals) < 100:
      print(len(self.time_vals))
      time.sleep(1) # wait until 100 values are gathered
      #todo add timeout
    
    left_time_2 = statistics.mean(self.time_vals[0])
    rigth_time_1 = statistics.mean(self.time_vals[1])
    self.calibration_sate += 1 # 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))

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

  def read(self):
    value = conn.getAcusticRTTs()
    return value

  def calculate_position(self,values):
    val1, val2 = values
    val1 -= self.overhead
    val2 -= self.overhead
    distance_left = val1 * self.sonic_speed / 1000 # millisecond -> mikrosecond value of distance is in mm
    distance_rigth = val2 * self.sonic_speed / 1000
    print(distance_left,distance_rigth)
    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)

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

if __name__ == "__main__":
  acc = AcusticSensor()
  acc.start()
  mag = MagneticSensor()
  mag.start()
  while True:
    time.sleep(1)