revert sensor config variables

raspberry-pi/sensors.py

@@ -13,17 +13,25 @@ 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"]
+    self.up_queue               = up_queue
+    self.down_queue             = down_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.calib_measurements = {
+    self.cal_values = {
       "left": [0, 0],
       "right": [0, 0] 
     }
+    self.n = 0
 
   def start(self):
     self.running = True
@@ -49,6 +57,8 @@ class AcusticSensor:
   def _readCb_dummy(self):
     while self.running:
       value = (900+random.randint(0,300),900+random.randint(0,300))
+      value = ((noise.pnoise1(self.n)+1)*150+900, (noise.pnoise1(self.n*1.3+3)+1)*150+900)
+      self.n += 0.01
       #print("dummy acc: ", value)
       if self.calibration_state.get_state() == self.calibration_state.ACCUMULATING_1:
         value = (1541+random.randint(-50,50),2076+random.randint(-50,50))
@@ -73,8 +83,8 @@ class AcusticSensor:
       self.time_vals[0].append(value[0])
       self.time_vals[1].append(value[1])
       if len(self.time_vals[0]) >= 100:
-        self.calib_measurements["left"][0]  = statistics.mean(self.time_vals[0])
-        self.calib_measurements["right"][1] = statistics.mean(self.time_vals[1])
+        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
 
@@ -82,32 +92,32 @@ class AcusticSensor:
       self.time_vals[0].append(value[0])
       self.time_vals[1].append(value[1])
       if len(self.time_vals[0]) >= 100:
-        self.calib_measurements["left"][1]  = statistics.mean(self.time_vals[0])
-        self.calib_measurements["right"][0] = statistics.mean(self.time_vals[1])
+        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.calib_measurements)
+        print(self.cal_values)
         
         # calculate calibration results
-        timedif = self.calib_measurements["left"][1] - self.calib_measurements["left"][0]
-        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 )
+        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.calib_measurements["left"][1] - distance_1/sonicspeed_1, self.calib_measurements["left"][0] - distance_2/sonicspeed_1))
+        overhead_1 = statistics.mean((self.cal_values["left"][1] - distance_1/sonicspeed_1, self.cal_values["left"][0] - distance_2/sonicspeed_1))
 
-        timedif = self.calib_measurements["right"][1] - self.calib_measurements["right"][0]
-        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 )
+        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.calib_measurements["right"][0] - distance_1/sonicspeed_2, self.calib_measurements["right"][1] - distance_2/sonicspeed_2))
+        overhead_2 = statistics.mean((self.cal_values["right"][0] - distance_1/sonicspeed_2, self.cal_values["right"][1] - distance_2/sonicspeed_2))
         print(distance_1,sonicspeed_1,distance_2,sonicspeed_2)
 
-        self.ac_conf["sonicspeed"] = str(statistics.mean((sonicspeed_1,sonicspeed_2)))
-        self.ac_conf["overhead_left"]  = str(overhead_1)
-        self.ac_conf["overhead_right"] = str(overhead_2)
-        print("calibration result", float(self.ac_conf["sonicspeed"]), float(self.ac_conf["overhead_left"]), float(self.ac_conf["overhead_right"]))
+        self.sonic_speed = statistics.mean((sonicspeed_1,sonicspeed_2))
+        self.overhead_left  = overhead_1
+        self.overhead_right = overhead_2
+        print("calibration result", self.sonic_speed, self.overhead_left, self.overhead_right)
         self.calibration_state.next_state()
 
   def read(self):
@@ -117,12 +127,12 @@ class AcusticSensor:
   def calculate_position(self,values):
     try:
       val1, val2 = values
-      val1 -= float(self.ac_conf["overhead_left"])
-      val2 -= float(self.ac_conf["overhead_right"])
-      distance_left = val1 * float(self.ac_conf["sonicspeed"])
-      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(max(distance_left**2 - x**2, 0)) + float(self.ac_conf["y_offset"])
+      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)