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

from connection import ArduinoSlave
import time
import statistics
import math

conn = ArduinoSlave()

class AcusticSensor:
  def __init__(self):
    self.sonic_speed_left = 0
    self.sonic_speed_right = 0
    self.sensor_distance = 450 #in mm

  def start(self):
    if not conn.isConnected():
      conn.open()
    conn.addRecvCallback(self._readCb)

  def _readCb(self, raw):
    print("acc: ", conn.getAcusticRTTs())

  def calibrate(self, distance):
    print("Move gondel to far left corner")
    input()
    speed = list()
    for i in range(100):
        time_val = [1.312,0]
        speed.append(distance/time_val[0])
    self.sonic_speed_left = statistics.mean(speed)
    print("sonic speed left:",self.sonic_speed_left)

    print("Move gondel to far right corner")
    input()
    speed = list()
    for i in range(100):
        time_val = [0,1.312]
        speed.append(distance/time_val[1])
    self.sonic_speed_right = statistics.mean(speed)
    print("sonic speed right:",self.sonic_speed_right)

  def read(self):
    return (952, 660)

  def calculate_position(self):
        if not self.sonic_speed_right or not self.sonic_speed_left:
            print("sensor not calibrated! calibrate now!")
            return (0,0)
        else:
            time_val = self.read()
            distance_left = time_val[0] * self.sonic_speed_left / 1000 # seconds -> mseconds
            distance_right = time_val[1] * self.sonic_speed_right / 1000
            print(distance_left,distance_right)
            x = (self.sensor_distance**2 - distance_right**2 + distance_left**2) / (2*self.sensor_distance)
            y = math.sqrt(distance_left**2 - x**2)
            return (x,y)

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)