RoboidControl-csharp/Unity/DifferentialDrive.cs

#if UNITY_5_3_OR_NEWER
using UnityEngine;

namespace RoboidControl.Unity {

    /// <summary>
    /// The Unity representation of a Roboid Control differential drive
    /// </summary>
    public class DifferentialDrive : Thing {

        /// <summary>
        /// The core differential drive
        /// </summary>
        public RoboidControl.DifferentialDrive coreDrive => core as RoboidControl.DifferentialDrive;

        /// <summary>
        /// Create the Unity representation
        /// </summary>
        /// <param name="coreDrive">The core touch sensor</param>
        /// <returns>The Unity representation of the touch sensor</returns>
        /// This uses a 'DifferentialDrive' resource when available for the Unity representation.
        /// If this is not available, a default representation is created.
        public static DifferentialDrive Create(RoboidControl.DifferentialDrive coreDrive) {
            DifferentialDrive differentialDrive;
            GameObject prefab = (GameObject)Resources.Load("DifferentialDrive");
            if (prefab != null) {
                // Use resource prefab when available
                GameObject gameObj = Instantiate(prefab);
                differentialDrive = gameObj.GetComponent<DifferentialDrive>();
                differentialDrive.Init(coreDrive);
            }
            else {
                // Default implementation
                GameObject gameObj = new(coreDrive.name);
                differentialDrive = gameObj.AddComponent<DifferentialDrive>();
                differentialDrive.Init(coreDrive);

                Rigidbody rb = gameObj.AddComponent<Rigidbody>();
                rb.isKinematic = false;
                rb.mass = 0.1f;
            }
            return differentialDrive;
        }

        /// <summary>
        /// The left wheel of the differential drive
        /// </summary>
        public Wheel leftWheel;
        /// <summary>
        /// The right wheel of the differential drive
        /// </summary>
        public Wheel rightWheel;
        /// <summary>
        /// The caster wheel to keep the differential drive horizontal
        /// </summary>
        public SphereCollider casterWheel;

        /// <summary>
        /// The rigidbody of the differential drive
        /// </summary>
        private Rigidbody rb = null;

        /// <summary>
        /// Start the Unity representation
        /// </summary>
        protected virtual void Start() {
            rb = GetComponent<Rigidbody>();
        }

        /// <summary>
        /// Handle the binary event indicating a configuration change
        /// </summary>
        protected override void HandleBinary() {
            // Ignore it when the wheel radius is not set
            if (coreDrive.wheelRadius <= 0)
                return;

            // Destroy any (generic) thing with the same id
            if (leftWheel == null) {
                Thing[] things = FindObjectsOfType<Thing>();
                foreach (Thing thing in things) {
                    if (thing.core.id == coreDrive.leftWheel.id)
                        Destroy(thing.gameObject);
                }
            }
            if (leftWheel == null && coreDrive.leftWheel != null)
                leftWheel = Wheel.Create(this.transform, coreDrive.leftWheel.id);
            if (leftWheel != null) {
                leftWheel.core ??= coreDrive.leftWheel;
                SphereCollider leftWheelCollider = leftWheel.GetComponent<SphereCollider>();
                leftWheelCollider.radius = coreDrive.wheelRadius;
            }

            // Destroy any (generic) thing with the same id
            if (rightWheel == null) {
                Thing[] things = FindObjectsOfType<Thing>();
                foreach (Thing thing in things) {
                    if (thing.core.id == coreDrive.rightWheel.id)
                        Destroy(thing.gameObject);
                }
            }
            if (rightWheel == null && coreDrive.rightWheel != null)
                rightWheel = Wheel.Create(this.transform, coreDrive.rightWheel.id);
            if (rightWheel != null) {
                rightWheel.core ??= coreDrive.rightWheel;
                SphereCollider rightWheelCollider = rightWheel.GetComponent<SphereCollider>();
                rightWheelCollider.radius = coreDrive.wheelRadius;
            }

            if (casterWheel == null) {
                GameObject gameObj = new("Caster wheel");
                gameObj.transform.parent = this.transform;
                casterWheel = gameObj.AddComponent<SphereCollider>();
                casterWheel.material = Wheel.slidingWheel;
            }
            casterWheel.radius = coreDrive.wheelRadius;
            // Put it in the middle of the back
            // This code assumes that the left wheel position has Direction.left and the right wheel Direction.right...
            float wheelSeparation = coreDrive.leftWheel.position.distance + coreDrive.rightWheel.position.distance;
            casterWheel.center = new Vector3(0, 0, -wheelSeparation);
        }

        /// <summary>
        /// Update the Unity representation state
        /// </summary>
        protected override void FixedUpdate() {
            base.FixedUpdate();

            if (rb != null && leftWheel != null && rightWheel != null) {
                float leftWheelVelocity = leftWheel.rotationSpeed * (2 * Mathf.PI) * coreDrive.wheelRadius;
                float rightWheelVelocity = rightWheel.rotationSpeed * (2 * Mathf.PI) * coreDrive.wheelRadius;

                // This code assumes that the left wheel position has Direction.left and the right wheel Direction.right...
                float wheelSeparation = coreDrive.leftWheel.position.distance + coreDrive.rightWheel.position.distance;
                float forwardSpeed = (leftWheelVelocity + rightWheelVelocity) / 2f;
                float turningSpeed = (leftWheelVelocity - rightWheelVelocity) / wheelSeparation;

                // Use smoothing to emulate motor inertia
                rb.velocity = 0.9f * rb.velocity + 0.1f * forwardSpeed * transform.forward;
                rb.angularVelocity = 0.9f * rb.angularVelocity + 0.1f * turningSpeed * Vector3.up;
            }
        }
    }

}
#endif