SensorBikeController.cs 6.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167
  1. using System;
  2. using System.Collections.Generic;
  3. using Controller.Bicycle;
  4. using Controller.Lean;
  5. using Study;
  6. using Sensors;
  7. using Sensors.ANT;
  8. using Sensors.Bluetooth;
  9. using Tracking;
  10. using System.Linq;
  11. using UnityEngine;
  12. // Declare outisde for public visibility
  13. //public enum SteeringMode { frontWheel, Leaning, HMD };
  14. namespace Controller
  15. {
  16. [Serializable]
  17. public struct FrontWheelTrackerConfig
  18. {
  19. public FrontWheelTracker frontWheelTracker;
  20. public float multiplicator;
  21. public float AdjustedRotation => frontWheelTracker.SteerRotation * multiplicator;
  22. }
  23. [Serializable]
  24. public struct HMDTrackerConfig
  25. {
  26. public HMDTracker cameraTracker;
  27. public float multiplicator;
  28. public float steeringOffset;
  29. public float AdjustedRotation => cameraTracker.SteerRotation * multiplicator;
  30. public float AdjustedLean => cameraTracker.LeanRotation * multiplicator;
  31. }
  32. [RequireComponent(typeof(IBicycleController))]
  33. public class SensorBikeController : MonoBehaviour
  34. {
  35. public PolarRotationMapping polarRotationMapping;
  36. public FrontWheelTrackerConfig frontWheelTrackerConfig;
  37. public HMDTrackerConfig hmdTrackerConfig;
  38. private float leanFactor;
  39. public bool steer = true;
  40. public bool accelerate = true;
  41. public bool lean = true;
  42. public Study.SteeringMode steeringSelection;
  43. private IBicycleController bicycleController;
  44. private bool isFrontWheelTrackerNotNull;
  45. private bool isHMDTrackerNotNull;
  46. private BikeSensorData sensorData;
  47. private Queue<float> previousLeanValues;
  48. public int framesToConsider = 10;
  49. public float tolerance = 5;
  50. private void Start()
  51. {
  52. isFrontWheelTrackerNotNull = frontWheelTrackerConfig.frontWheelTracker != null;
  53. isHMDTrackerNotNull = hmdTrackerConfig.cameraTracker != null;
  54. bicycleController = GetComponent<IBicycleController>();
  55. sensorData = BikeSensorData.Instance;
  56. //leanFactor = 90f / (polarRotationMapping.maxRight - polarRotationMapping.center);
  57. leanFactor = 45f / polarRotationMapping.maxRight;
  58. if (framesToConsider > 1) previousLeanValues = new Queue<float>(framesToConsider);
  59. // Sort of callibration for polar Sensor
  60. var polarData = sensorData.BleData;
  61. if (steeringSelection == Study.SteeringMode.Leaning && polarData != null)
  62. {
  63. Debug.Log("Calibration of Polar Sensor");
  64. polarRotationMapping.center = polarData.Value.Acc.y;
  65. }
  66. }
  67. private void Update()
  68. {
  69. Debug.Log("Bike Sensor Controller called");
  70. var speedData = sensorData.SpeedData;
  71. if (speedData != null && accelerate) SetSpeed(speedData.Value);
  72. if (steer) SetSteer();
  73. if (lean) SetLeaningAngle();
  74. }
  75. public void SetCondition(Study.SteeringMode mode)
  76. {
  77. Debug.Log("Setting Condition");
  78. steeringSelection = mode;
  79. }
  80. private void SetSteer()
  81. {
  82. switch (steeringSelection)
  83. {
  84. case Study.SteeringMode.frontWheel:
  85. if (isFrontWheelTrackerNotNull)
  86. {
  87. bicycleController.CurrentSteerAngle = frontWheelTrackerConfig.AdjustedRotation * 2f;
  88. }
  89. break;
  90. case Study.SteeringMode.Leaning:
  91. var polarData = sensorData.BleData;
  92. if (polarData != null)
  93. {
  94. Debug.Log("Updating Polar Data");
  95. Debug.Log("Polar Y: " + polarData.Value.Acc);
  96. var polarLean = CalculateLeanRotationMultiFrame(polarData);
  97. Debug.Log("Ploar Post-Processed: " + polarLean);
  98. bicycleController.CurrentSteerAngle = polarLean;
  99. // Activate below in case we also need the steering angle
  100. //bicycleController.CurrentSteerAngle = (polarData.Value.Acc.y - polarRotationMapping.center) * leanFactor;
  101. }
  102. break;
  103. case Study.SteeringMode.HMD:
  104. if (isHMDTrackerNotNull)
  105. {
  106. // We emprirically observed a right-drift
  107. // We subtract a constant to counteract this
  108. bicycleController.CurrentSteerAngle = hmdTrackerConfig.AdjustedRotation;
  109. }
  110. break;
  111. }
  112. Debug.Log("Updating Steering Angle to " + bicycleController.CurrentSteerAngle);
  113. }
  114. private void SetLeaningAngle()
  115. {
  116. //bicycleController.CurrentLeaningAngle =
  117. }
  118. private void SetSpeed(SpeedSensorData speedData)
  119. {
  120. bicycleController.CurrentSpeed = speedData.Speed;
  121. }
  122. // Helpers for Lean Steering
  123. private float CalculateLeanRotationSingleFrame(BleSensorData? polarData)
  124. {
  125. float steerUpdate = polarData.Value.Acc.y - polarRotationMapping.center;
  126. //steerUpdate = Math.Abs(steerUpdate - polarRotationMapping.center) < tolerance ? 0 : steerUpdate;
  127. return steerUpdate * leanFactor;
  128. }
  129. private float CalculateLeanRotationMultiFrame(BleSensorData? polarData)
  130. {
  131. var angleThisFrame = CalculateLeanRotationSingleFrame(polarData);
  132. if (previousLeanValues.Count > framesToConsider) previousLeanValues.Dequeue();
  133. previousLeanValues.Enqueue(angleThisFrame);
  134. // Use exponential moving average
  135. float alpha = 2f / (framesToConsider + 1);
  136. float steerUpdate = previousLeanValues.DefaultIfEmpty()
  137. .Aggregate((ema, nextQuote) => alpha * nextQuote + (1 - alpha) * ema);
  138. // Do Global Thresholding, to avoid noisy steering
  139. return Math.Abs(steerUpdate - polarRotationMapping.center) < tolerance ? 0 : steerUpdate;
  140. //return steerUpdate;
  141. }
  142. }
  143. }