SensorBikeController.cs 3.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105
  1. using System;
  2. using UnityEngine;
  3. using UnityEngine.Serialization;
  4. [Serializable]
  5. public struct PolarRotationMapping
  6. {
  7. public float maxRight;
  8. public float center;
  9. }
  10. public class SensorBikeController : MonoBehaviour
  11. {
  12. public float maxSpeed = 40f;
  13. public float maxMotorTorque = 1000;
  14. public PolarRotationMapping polarRotationMapping;
  15. public SpeedSensorConfig speedSensorConfig;
  16. public PolarSensorConfig polarSensorConfig;
  17. public FrontWheelTracker frontWheelTracker;
  18. public bool steer = true;
  19. public bool accelerate = true;
  20. public bool lean = true;
  21. private BicycleController bicycleController;
  22. private BikeSensorData sensorData;
  23. private float leanFactor;
  24. private bool isFrontWheelTrackerNotNull;
  25. private async void Start()
  26. {
  27. isFrontWheelTrackerNotNull = frontWheelTracker != null;
  28. bicycleController = GetComponent<BicycleController>();
  29. sensorData = BikeSensorData.Instance;
  30. await sensorData.StartListening(polarSensorConfig: polarSensorConfig, speedSensorConfig: speedSensorConfig);
  31. leanFactor = 90f / (polarRotationMapping.maxRight - polarRotationMapping.center);
  32. }
  33. private void Update()
  34. {
  35. var speedData = sensorData.SpeedData;
  36. var polarData = sensorData.PolarData;
  37. if (speedData != null && accelerate)
  38. {
  39. SetSpeed(speedData.Value);
  40. }
  41. if (isFrontWheelTrackerNotNull && steer)
  42. {
  43. SetSteer();
  44. }
  45. if (polarData != null && lean)
  46. {
  47. SetLeaningAngle(polarData.Value);
  48. }
  49. }
  50. private void SetSteer()
  51. {
  52. bicycleController.CurrentSteerAngle = frontWheelTracker.Rotation.y; //TODO: something a bit smarter
  53. }
  54. private void OnDestroy()
  55. {
  56. sensorData.Dispose();
  57. }
  58. private void SetLeaningAngle(PolarSensorData polarData)
  59. {
  60. //don't lean while standing / walking to bike
  61. if (bicycleController.rigidBody.velocity.magnitude > .5f)
  62. {
  63. bicycleController.CurrentLeaningAngle = (polarData.Acc.y - polarRotationMapping.center) * leanFactor;
  64. }
  65. }
  66. private void SetSpeed(SpeedSensorData speedData)
  67. {
  68. var currentSpeed = bicycleController.rigidBody.velocity.magnitude;
  69. var speedDif = speedData.Speed - currentSpeed;
  70. var ratio = speedDif / maxSpeed;
  71. var torque = maxMotorTorque * ratio;
  72. if (speedDif >= .1f) // 0.36 km/h
  73. {
  74. Debug.Log($"SpeedDif = {speedDif} -> applying Torque {torque} (Ratio: {ratio})");
  75. bicycleController.CurrentBrakeTorque = 0;
  76. bicycleController.CurrentMotorTorque = torque;
  77. }
  78. else if (speedDif <= -.1f)
  79. {
  80. Debug.Log($"SpeedDif = {speedDif} -> applying brake Torque {torque} (Ratio: {ratio})");
  81. bicycleController.CurrentMotorTorque = 0;
  82. bicycleController.CurrentBrakeTorque = -torque;
  83. }
  84. // without else the speed overshoots a bit, but is way closer to real speed
  85. /*else
  86. {
  87. bicycleController.CurrentMotorTorque = 0;
  88. bicycleController.CurrentBrakeTorque = 0;
  89. }*/
  90. }
  91. }