SensorBikeController.cs 2.7 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697
  1. using System;
  2. using Controller.Bicycle;
  3. using Sensors;
  4. using Sensors.ANT;
  5. using Sensors.Polar;
  6. using Tracking;
  7. using UnityEngine;
  8. using UnityEngine.Serialization;
  9. namespace Controller
  10. {
  11. [Serializable]
  12. public struct PolarRotationMapping
  13. {
  14. public float maxRight;
  15. public float center;
  16. }
  17. [Serializable]
  18. public struct FrontWheelTrackerConfig
  19. {
  20. public FrontWheelTracker frontWheelTracker;
  21. public float multiplicator;
  22. public float AdjustedRotation => frontWheelTracker.SteerRotation * multiplicator;
  23. }
  24. [RequireComponent(typeof(IBicycleController))]
  25. public class SensorBikeController : MonoBehaviour
  26. {
  27. public PolarRotationMapping polarRotationMapping;
  28. public SpeedSensorConfig speedSensorConfig;
  29. public PolarSensorConfig polarSensorConfig;
  30. public FrontWheelTrackerConfig frontWheelTrackerConfig;
  31. public bool steer = true;
  32. public bool accelerate = true;
  33. public bool lean = true;
  34. private IBicycleController bicycleController;
  35. private BikeSensorData sensorData;
  36. private float leanFactor;
  37. private bool isFrontWheelTrackerNotNull;
  38. private void Start()
  39. {
  40. isFrontWheelTrackerNotNull = frontWheelTrackerConfig.frontWheelTracker != null;
  41. bicycleController = GetComponent<IBicycleController>();
  42. sensorData = BikeSensorData.Instance;
  43. sensorData.StartListening(polarSensorConfig: polarSensorConfig, speedSensorConfig: speedSensorConfig);
  44. leanFactor = 90f / (polarRotationMapping.maxRight - polarRotationMapping.center);
  45. }
  46. private void Update()
  47. {
  48. var speedData = sensorData.SpeedData;
  49. var polarData = sensorData.PolarData;
  50. if (speedData != null && accelerate)
  51. {
  52. SetSpeed(speedData.Value);
  53. }
  54. if (isFrontWheelTrackerNotNull && steer)
  55. {
  56. SetSteer();
  57. }
  58. if (polarData != null && lean)
  59. {
  60. SetLeaningAngle(polarData.Value);
  61. }
  62. }
  63. private void SetSteer()
  64. {
  65. bicycleController.CurrentSteerAngle =
  66. frontWheelTrackerConfig.AdjustedRotation;
  67. }
  68. private void OnDestroy()
  69. {
  70. sensorData?.Dispose();
  71. }
  72. private void SetLeaningAngle(PolarSensorData polarData)
  73. {
  74. bicycleController.CurrentLeaningAngle = (polarData.Acc.y - polarRotationMapping.center) * leanFactor;
  75. }
  76. private void SetSpeed(SpeedSensorData speedData)
  77. {
  78. bicycleController.CurrentSpeed = speedData.Speed;
  79. }
  80. }
  81. }