HoHCI-BikeSim/Assets/Scripts/BicyleController/SensorBikeController.cs

using System;
using UnityEngine;
using UnityEngine.Serialization;

[Serializable]
public struct PolarRotationMapping
{
    public float maxRight;
    public float center;
}

[Serializable]
public struct FrontWheelTrackerConfig
{
    public FrontWheelTracker frontWheelTracker;
    public float multiplicator;

    public Vector3 AdjustedRotation => frontWheelTracker.Rotation * multiplicator;
}

public class SensorBikeController : MonoBehaviour
{
    public float maxSpeed = 40f;
    public float maxMotorTorque = 1000;
    public PolarRotationMapping polarRotationMapping;

    public SpeedSensorConfig speedSensorConfig;
    public PolarSensorConfig polarSensorConfig;
    public FrontWheelTrackerConfig frontWheelTrackerConfig;

    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 = frontWheelTrackerConfig.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 = frontWheelTrackerConfig.AdjustedRotation.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;
        }*/
    }
}