123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105 |
- using System;
- using UnityEngine;
- using UnityEngine.Serialization;
- [Serializable]
- public struct PolarRotationMapping
- {
- public float maxRight;
- public float center;
- }
- public class SensorBikeController : MonoBehaviour
- {
- public float maxSpeed = 40f;
- public float maxMotorTorque = 1000;
- public PolarRotationMapping polarRotationMapping;
- public SpeedSensorConfig speedSensorConfig;
- public PolarSensorConfig polarSensorConfig;
- public FrontWheelTracker frontWheelTracker;
- public bool steer = true;
- public bool accelerate = true;
- public bool lean = true;
- private BicycleController bicycleController;
- private BikeSensorData sensorData;
- private float leanFactor;
- private bool isFrontWheelTrackerNotNull;
- private async void Start()
- {
- isFrontWheelTrackerNotNull = frontWheelTracker != null;
- bicycleController = GetComponent<BicycleController>();
- sensorData = BikeSensorData.Instance;
- await sensorData.StartListening(polarSensorConfig: polarSensorConfig, speedSensorConfig: speedSensorConfig);
- leanFactor = 90f / (polarRotationMapping.maxRight - polarRotationMapping.center);
- }
- private void Update()
- {
- var speedData = sensorData.SpeedData;
- var polarData = sensorData.PolarData;
- if (speedData != null && accelerate)
- {
- SetSpeed(speedData.Value);
- }
- if (isFrontWheelTrackerNotNull && steer)
- {
- SetSteer();
- }
- if (polarData != null && lean)
- {
- SetLeaningAngle(polarData.Value);
- }
- }
- private void SetSteer()
- {
- bicycleController.CurrentSteerAngle = frontWheelTracker.Rotation.y; //TODO: something a bit smarter
- }
- private void OnDestroy()
- {
- sensorData.Dispose();
- }
- private void SetLeaningAngle(PolarSensorData polarData)
- {
- //don't lean while standing / walking to bike
- if (bicycleController.rigidBody.velocity.magnitude > .5f)
- {
- bicycleController.CurrentLeaningAngle = (polarData.Acc.y - polarRotationMapping.center) * leanFactor;
- }
- }
- private void SetSpeed(SpeedSensorData speedData)
- {
- var currentSpeed = bicycleController.rigidBody.velocity.magnitude;
- var speedDif = speedData.Speed - currentSpeed;
- var ratio = speedDif / maxSpeed;
- var torque = maxMotorTorque * ratio;
- if (speedDif >= .1f) // 0.36 km/h
- {
- Debug.Log($"SpeedDif = {speedDif} -> applying Torque {torque} (Ratio: {ratio})");
- bicycleController.CurrentBrakeTorque = 0;
- bicycleController.CurrentMotorTorque = torque;
- }
- else if (speedDif <= -.1f)
- {
- Debug.Log($"SpeedDif = {speedDif} -> applying brake Torque {torque} (Ratio: {ratio})");
- bicycleController.CurrentMotorTorque = 0;
- bicycleController.CurrentBrakeTorque = -torque;
- }
- // without else the speed overshoots a bit, but is way closer to real speed
- /*else
- {
- bicycleController.CurrentMotorTorque = 0;
- bicycleController.CurrentBrakeTorque = 0;
- }*/
- }
- }
|