|
|
@@ -1,12 +1,10 @@
|
|
|
+from distutils.command.config import config
|
|
|
import queue
|
|
|
import statistics
|
|
|
from struct import calcsize
|
|
|
import numpy as np
|
|
|
import time
|
|
|
-import threading # ?
|
|
|
-import random # ?
|
|
|
from configparser import ConfigParser
|
|
|
-
|
|
|
from sensors.calibration import CalibrationStateMashine
|
|
|
from sensors.connection import globalArduinoSlave
|
|
|
import logHandler
|
|
|
@@ -20,59 +18,95 @@ class MagneticSensor:
|
|
|
self.queue = queue.Queue()
|
|
|
self.calibration_state = CalibrationStateMashine()
|
|
|
self.success = False
|
|
|
- self.mag_offset_x = float(conf["mag_sensor"]["mag_offset_x"])
|
|
|
- self.mag_offset_y = float(conf["mag_sensor"]["mag_offset_y"])
|
|
|
+ self.offset_x = float(conf["mag_sensor"]["off_x"])
|
|
|
+ self.offset_y = float(conf["mag_sensor"]["off_y"])
|
|
|
+ self.scale_x = float(conf["mag_sensor"]["scale_x"])
|
|
|
+ self.scale_y = float(conf["mag_sensor"]["scale_y"])
|
|
|
+ self.max_x = float(conf["mag_sensor"]["max_x"])
|
|
|
+ self.max_y = float(conf["mag_sensor"]["max_y"])
|
|
|
self.log_handler = logHandler.get_log_handler()
|
|
|
|
|
|
def start(self):
|
|
|
if not conn.isConnected():
|
|
|
conn.open()
|
|
|
+ self.success = True
|
|
|
conn.addRecvCallback(self._readCb)
|
|
|
|
|
|
def _readCb(self, raw):
|
|
|
- mag_values = conn.getMagneticField()
|
|
|
- print("Magnetic Offsets:", conn.getMagneticOffsets())
|
|
|
- if mag_values[0] >= 0 and mag_values[1] >= 0:
|
|
|
- pass
|
|
|
- #position = self.calculate_position(mag_values) ### MUSS AUF MAG. SENSOR ANGEPASST WERDEN!!!
|
|
|
- #if position != None:
|
|
|
- # self.pass_to_gui(position + value)
|
|
|
-
|
|
|
-
|
|
|
- def setOffsets(self):
|
|
|
- # Read config file
|
|
|
- config_object = ConfigParser()
|
|
|
- config_object("config.ini")
|
|
|
- # Get mag_sensor section
|
|
|
- mag_sensor = config_object["mag_sensor"]
|
|
|
- # Update Offsets
|
|
|
- mag_sensor["offset_x"] = conn.getMagneticOffsets(0)
|
|
|
- mag_sensor["offset_y"] = conn.getMagneticOffsets(1)
|
|
|
-
|
|
|
-
|
|
|
+ value = conn.getMagneticField()
|
|
|
+ if value[0] >= 0 and value[1] >= 0:
|
|
|
+ self.calibrate(value)
|
|
|
+ position = self.calculate_position(value)
|
|
|
+ if position != None:
|
|
|
+ self.pass_to_gui(position + value)
|
|
|
+
|
|
|
def start_calibration(self): ###
|
|
|
self.calibration_state.reset_state()
|
|
|
- self.time_vals = [[],[]]
|
|
|
+ self.time_vals = [[],[],[]]
|
|
|
self.calibration_state.next_state()
|
|
|
|
|
|
- def calibrate(self, value): ### öffnet erstmal popup :)
|
|
|
- #pass
|
|
|
+ def calibrate(self, value): ### öffnet erstmal popup :) # how?
|
|
|
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])
|
|
|
+ self.time_vals[2].append(value[2])
|
|
|
self.calibration_state.progress = len(self.time_vals[0]) / 2
|
|
|
if len(self.time_vals[0]) >= 100:
|
|
|
self.cal_values["front"][0] = statistics.mean(self.time_vals[0])
|
|
|
self.cal_values["front"][1] = statistics.mean(self.time_vals[1])
|
|
|
- self.time_vals = [[],[]]
|
|
|
+ self.cal_values["front"][2] = statistics.mean(self.time_vals[2])
|
|
|
+ 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])
|
|
|
+ self.time_vals[2].append(value[2])
|
|
|
+ self.calibration_state.progress = 50 + len(self.time_vals[0]) / 2
|
|
|
+ if len(self.time_vals[0]) >= 100:
|
|
|
+ self.cal_values["back"][0] = statistics.mean(self.time_vals[0])
|
|
|
+ self.cal_values["back"][1] = statistics.mean(self.time_vals[1])
|
|
|
+ self.cal_values["back"][2] = statistics.mean(self.time_vals[2])
|
|
|
+
|
|
|
+ # all values have been captured
|
|
|
+
|
|
|
+ # Hard iron distortion & Soft iron distortion
|
|
|
+ self.offset_x = (self.cal_values["back"][0] - self.cal_values["front"][0]) / 2
|
|
|
+ self.offset_y = (self.cal_values["back"][1] - self.cal_values["front"][1]) / 2
|
|
|
+
|
|
|
+ avg_delta_x = (self.cal_values["back"][0] - self.cal_values["front"][0]) / 2
|
|
|
+ avg_delta_y = (self.cal_values["back"][1] - self.cal_values["front"][1]) / 2
|
|
|
+ avg_delta_z = (self.cal_values["back"][2] - self.cal_values["front"][2]) / 2
|
|
|
+
|
|
|
+ avg_delta = (avg_delta_x + avg_delta_y + avg_delta_z) / 3
|
|
|
+
|
|
|
+ self.scale_x = avg_delta / avg_delta_x
|
|
|
+ self.scale_y = avg_delta / avg_delta_y
|
|
|
+ self.scale_z = avg_delta / avg_delta_z
|
|
|
+
|
|
|
+ # max values for placeholder algorithm
|
|
|
+ self.max_x = (self.cal_values["back"][0] - self.offset_x) * self.scale_x
|
|
|
+ self.max_y = (self.cal_values["back"][1] - self.offset_y) * self.scale_y
|
|
|
+
|
|
|
+ self.calibration_state.next_state()
|
|
|
+
|
|
|
+ return self.offset_x, self.offset_y, self.scale_x, self.scale_y, self.max_x, self.max_y
|
|
|
|
|
|
def read(self):
|
|
|
- return conn.getMagneticField()
|
|
|
+ value = conn.getMagneticField()
|
|
|
+ return value
|
|
|
+
|
|
|
+ def calculate_position(self): ###
|
|
|
+ corrected_x = (conn.getMagneticField[0] - self.offset_x) * self.scale_x
|
|
|
+ corrected_y = (conn.getMagneticField[1] - self.offset_y) * self.scale_y
|
|
|
+ # placeholder algorithm (to see if the sensor even works)
|
|
|
+ x = (corrected_x * 400) / self.max_x
|
|
|
+ y = (corrected_y * 400) / self.max_y
|
|
|
+ return (x, y)
|
|
|
|
|
|
def stop(self):
|
|
|
self.log_handler.log_and_print("stop magnetic sensor")
|
|
|
+ self.success = False
|
|
|
conn.close()
|
|
|
|
|
|
def pass_to_gui(self, data):
|
