Assets/Scripts/Controller/SensorBikeController.cs

@@ -55,7 +55,9 @@ namespace Controller
 
         private Queue<float> previousLeanValues;
         public int framesToConsider = 10;
-        public float tolerance = 5;
+        public float tolerance_global = 5;
+        public float tolerance_local = 7;
+        public int claibrationcount = 10;
 
         private GameObject bikePlayer;
         private void Start()
@@ -65,18 +67,32 @@ namespace Controller
             bicycleController = GetComponent<IBicycleController>();
             sensorData = BikeSensorData.Instance;
             //leanFactor = 90f / (polarRotationMapping.maxRight - polarRotationMapping.center);
-            leanFactor = 45f / polarRotationMapping.maxRight;
+            leanFactor = 30f / polarRotationMapping.maxRight; //von 45 auf 30 angepasst
             if (framesToConsider > 1) previousLeanValues = new Queue<float>(framesToConsider);
 
-            // Sort of callibration for polar Sensor
             var polarData = sensorData.BleData;
             if (steeringSelection == Study.SteeringMode.Leaning && polarData != null)
             {
-                Debug.Log("Calibration of Polar Sensor");
-                polarRotationMapping.center = polarData.Value.Acc.y;
+                CalibratePolar()
+                //polarRotationMapping.center = polarData.Value.Acc.y;
             }
 
             bikePlayer = GameObject.Find("bike");
+            Debug.Log("Default fpr previousLeanValues " + previousLeanValues.DefaultIfEmpty());
+        }
+
+        private void CalibratePolar()
+        {
+            Debug.Log("Calibration of Polar Sensor");
+            Queue<float> calibrationLeanValues;
+            calibrationLeanValues = new Queue<float>(claibrationcount);
+            for (int i = 0; i < claibrationcount; i++)
+            {
+                float pos = sensorData.BleData.Value.Acc.y;
+                calibrationLeanValues.Enqueue(pos);
+                yield return new WaitForSeconds(0.25f); //evtl. new loeschen
+            }
+            polarRotationMapping.center = calibrationLeanValues.Average();
         }
 
         private void Update()
@@ -155,10 +171,13 @@ namespace Controller
         private float CalculateLeanRotationSingleFrame(BleSensorData? polarData)
         {
             float steerUpdate = polarData.Value.Acc.y - polarRotationMapping.center;
-            //steerUpdate = Math.Abs(steerUpdate - polarRotationMapping.center) < tolerance ? 0 : steerUpdate;
+            //lokal average:
+            //steerUpdate = Math.Abs(steerUpdate - polarRotationMapping.center) < tolerance_local ? 0 : steerUpdate;
+            //steerUpdate = Math.Pow(steerUpdate - polarRotationMapping.center, 2) < 30 ? 0 : steerUpdate;
             return steerUpdate * leanFactor;
         }
 
+
         private float CalculateLeanRotationMultiFrame(BleSensorData? polarData)
         {
             var angleThisFrame = CalculateLeanRotationSingleFrame(polarData);
@@ -166,13 +185,20 @@ namespace Controller
 
             previousLeanValues.Enqueue(angleThisFrame);
 
+            //neue-Option:
+            //float steerOld = previousLeanValues.Peek();
+            //float mean = previousLeanValues.Average();
+            //steerUpdate = Math.Pow(steerUpdate - mean, 2) < tolerance_global ? steerOld : steerUpdate; 
+
+            //alte-Option:
+
             // Use exponential moving average
             float alpha = 2f / (framesToConsider + 1);
-            float steerUpdate = previousLeanValues.DefaultIfEmpty()
+            float steerUpdate = previousLeanValues.DefaultIfEmpty(polarRotationMapping.center) //angepasst: Default ist nun center
              .Aggregate((ema, nextQuote) => alpha * nextQuote + (1 - alpha) * ema);
 
             // Do Global Thresholding, to avoid noisy steering
-            return Math.Abs(steerUpdate - polarRotationMapping.center) < tolerance ? 0 : steerUpdate;
+            return Math.Abs(steerUpdate - polarRotationMapping.center) < tolerance_global ? 0 : steerUpdate;
             //return steerUpdate;
         }
     }