122 lines
5.4 KiB
C#
122 lines
5.4 KiB
C#
#if UNITY_5_3_OR_NEWER
|
|
using UnityEngine;
|
|
|
|
namespace RoboidControl.Unity {
|
|
public class DifferentialDrive : Thing {
|
|
|
|
public Wheel leftWheel;
|
|
public Wheel rightWheel;
|
|
public SphereCollider casterWheel;
|
|
|
|
protected RoboidControl.DifferentialDrive coreDrive => core as RoboidControl.DifferentialDrive;
|
|
|
|
/// <summary>
|
|
/// Create the Unity representation
|
|
/// </summary>
|
|
/// <param name="core">The core touch sensor</param>
|
|
/// <returns>The Unity representation of the touch sensor</returns>
|
|
public static DifferentialDrive Create(RoboidControl.DifferentialDrive core) {
|
|
DifferentialDrive component = null;
|
|
Rigidbody rb = null;
|
|
GameObject prefab = (GameObject)Resources.Load("DifferentialDrive");
|
|
if (prefab != null) {
|
|
// Use resource prefab when available
|
|
GameObject gameObj = Instantiate(prefab);
|
|
component = gameObj.GetComponent<DifferentialDrive>();
|
|
if (component != null)
|
|
component.core = core;
|
|
|
|
rb = component.GetComponent<Rigidbody>();
|
|
}
|
|
else {
|
|
// Fallback implementation
|
|
GameObject gameObj = new(core.name);
|
|
component = gameObj.AddComponent<DifferentialDrive>();
|
|
component.Init(core);
|
|
|
|
rb = gameObj.AddComponent<Rigidbody>();
|
|
rb.isKinematic = false;
|
|
rb.mass = 0.1f;
|
|
}
|
|
return component;
|
|
}
|
|
|
|
private Rigidbody rb = null;
|
|
|
|
protected virtual void Start() {
|
|
rb = GetComponent<Rigidbody>();
|
|
}
|
|
|
|
protected override void HandleBinary() {
|
|
Debug.Log("Diff drive handle Binary");
|
|
if (coreDrive.wheelRadius <= 0) // || coreDrive.wheelSeparation <= 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;
|
|
// leftWheelCollider.center = new Vector3(-coreDrive.wheelSeparation / 2, 0, 0);
|
|
}
|
|
|
|
// 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;
|
|
// rightWheelCollider.center = new Vector3(coreDrive.wheelSeparation / 2, 0, 0);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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 |