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

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.up_queue = up_queue
    self.down_queue = down_queue
    self.calibration_state = calibration_state
    self.conf = conf
    self.ac_conf = conf["ac_sensor"]

  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])
    right_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])
    right_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(float(self.ac_conf["left_x_offset"])**2 + (float(self.ac_conf["y_offset"]) + float(self.conf["field"]["y"]))**2 )
    distance_2 = math.sqrt((float(self.ac_conf["left_x_offset"]) + float(self.conf["field"]["x"])**2 + (float(self.ac_conf["y_offset"]) + float(self.conf["field"]["y"])))**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 = right_time_2 - right_time_1
    distance_1 = math.sqrt(float(self.ac_conf["right_x_offset"])**2 + (float(self.ac_conf["y_offset"]) + float(self.conf["field"]["y"]))**2 )
    distance_2 = math.sqrt((float(self.ac_conf["right_x_offset"]) + float(self.conf["field"]["x"])**2 + (float(self.ac_conf["y_offset"]) + float(self.conf["field"]["y"])))**2 )
    distancedif = distance_2 - distance_1
    sonicspeed_2 = distancedif / timedif
    overhead_2 = statistics.mean((right_time_1 - distance_1/sonicspeed_2,right_time_2 - distance_2/sonicspeed_2))

    self.ac_conf["sonicspeed"] = str(statistics.mean((sonicspeed_1,sonicspeed_2)))
    self.ac_conf["overhead"]   = str(statistics.mean((overhead_1,overhead_2)))
    print("calibration result",float(self.ac_conf["sonicspeed"]),float(self.ac_conf["overhead"]))
    self.calibration_state.next_state()

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

  def calculate_position(self,values):
    try:
      val1, val2 = values
      val1 -= float(self.ac_conf["overhead"])
      val2 -= float(self.ac_conf["overhead"])
      distance_left = val1 * float(self.ac_conf["sonicspeed"]) # millisecond -> mikrosecond value of distance is in mm
      distance_right = val2 * float(self.ac_conf["sonicspeed"])
      x = (float(self.ac_conf["sensor_distance"])**2 - distance_right**2 + distance_left**2) / (2*float(self.ac_conf["sensor_distance"])) + float(self.ac_conf["left_x_offset"])
      y = math.sqrt(abs(distance_left**2 - x**2)) + float(self.ac_conf["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)