HoHCI-BikeSteering/Assets/Scripts/Controller/SensorBikeController.cs

using System;
using Sensors;
using Sensors.ANT;
using Sensors.Polar;
using Tracking;
using UnityEngine;
using UnityEngine.Serialization;

namespace Controller
{
    [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;
        }*/
        }
    }
}