|
@@ -55,7 +55,9 @@ namespace Controller
|
|
|
|
|
|
private Queue<float> previousLeanValues;
|
|
private Queue<float> previousLeanValues;
|
|
public int framesToConsider = 10;
|
|
public int framesToConsider = 10;
|
|
- public float tolerance = 5;
|
|
|
|
|
|
+ public float tolerance_global = 5;
|
|
|
|
+ public float tolerance_local = 7;
|
|
|
|
+ public int claibrationcount = 10;
|
|
|
|
|
|
private void Start()
|
|
private void Start()
|
|
{
|
|
{
|
|
@@ -64,16 +66,31 @@ namespace Controller
|
|
bicycleController = GetComponent<IBicycleController>();
|
|
bicycleController = GetComponent<IBicycleController>();
|
|
sensorData = BikeSensorData.Instance;
|
|
sensorData = BikeSensorData.Instance;
|
|
//leanFactor = 90f / (polarRotationMapping.maxRight - polarRotationMapping.center);
|
|
//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);
|
|
if (framesToConsider > 1) previousLeanValues = new Queue<float>(framesToConsider);
|
|
|
|
|
|
- // Sort of callibration for polar Sensor
|
|
|
|
var polarData = sensorData.BleData;
|
|
var polarData = sensorData.BleData;
|
|
if (steeringSelection == Study.SteeringMode.Leaning && polarData != null)
|
|
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;
|
|
}
|
|
}
|
|
|
|
+ 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()
|
|
private void Update()
|
|
@@ -143,10 +160,13 @@ namespace Controller
|
|
private float CalculateLeanRotationSingleFrame(BleSensorData? polarData)
|
|
private float CalculateLeanRotationSingleFrame(BleSensorData? polarData)
|
|
{
|
|
{
|
|
float steerUpdate = polarData.Value.Acc.y - polarRotationMapping.center;
|
|
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;
|
|
return steerUpdate * leanFactor;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
+
|
|
private float CalculateLeanRotationMultiFrame(BleSensorData? polarData)
|
|
private float CalculateLeanRotationMultiFrame(BleSensorData? polarData)
|
|
{
|
|
{
|
|
var angleThisFrame = CalculateLeanRotationSingleFrame(polarData);
|
|
var angleThisFrame = CalculateLeanRotationSingleFrame(polarData);
|
|
@@ -154,13 +174,20 @@ namespace Controller
|
|
|
|
|
|
previousLeanValues.Enqueue(angleThisFrame);
|
|
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
|
|
// Use exponential moving average
|
|
float alpha = 2f / (framesToConsider + 1);
|
|
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);
|
|
.Aggregate((ema, nextQuote) => alpha * nextQuote + (1 - alpha) * ema);
|
|
|
|
|
|
// Do Global Thresholding, to avoid noisy steering
|
|
// 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;
|
|
//return steerUpdate;
|
|
}
|
|
}
|
|
}
|
|
}
|