73 lines
2.6 KiB
C#
73 lines
2.6 KiB
C#
#if UNITY_5_3_OR_NEWER
|
|
using System.Collections;
|
|
using UnityEngine;
|
|
using LinearAlgebra;
|
|
|
|
namespace RoboidControl.Unity {
|
|
public class RelativeEncoder : Thing {
|
|
|
|
/// <summary>
|
|
/// Create the Unity representation
|
|
/// </summary>
|
|
/// <param name="core">The core motor</param>
|
|
/// <returns>The Unity representation of a motor</returns>
|
|
public static RelativeEncoder Create(RoboidControl.RelativeEncoder core) {
|
|
GameObject prefab = (GameObject)Resources.Load("IncrementalEncoder");
|
|
if (prefab != null) {
|
|
// Use resource prefab when available
|
|
GameObject gameObj = Instantiate(prefab);
|
|
RelativeEncoder component = gameObj.GetComponent<RelativeEncoder>();
|
|
if (component != null)
|
|
component.core = core;
|
|
return component;
|
|
}
|
|
else {
|
|
// Fallback implementation
|
|
GameObject gameObj = new(core.name);
|
|
RelativeEncoder component = gameObj.AddComponent<RelativeEncoder>();
|
|
component.Init(core);
|
|
|
|
Rigidbody rb = gameObj.AddComponent<Rigidbody>();
|
|
rb.isKinematic = true;
|
|
return component;
|
|
}
|
|
}
|
|
RoboidControl.RelativeEncoder coreEncoder => this.core as RoboidControl.RelativeEncoder;
|
|
|
|
/// <summary>
|
|
/// The rotation speed in degrees per second
|
|
/// </summary>
|
|
public float rotationSpeed = 0.0f;
|
|
|
|
protected override void HandleBinary() {
|
|
this.rotationSpeed = coreEncoder.angularSpeed;
|
|
}
|
|
|
|
private Quaternion lastRotation;
|
|
protected override void Update() {
|
|
base.Update();
|
|
if (this.transform.childCount == 0)
|
|
return;
|
|
|
|
Transform child = this.transform.GetChild(0);
|
|
|
|
// Get the delta rotation since last frame
|
|
Quaternion deltaRotation = Quaternion.Inverse(lastRotation) * child.localRotation;
|
|
|
|
// Normalize angles to range [-180..180)
|
|
// Note: it is not possible to track rotation speeds higher than 180 degrees per frame because of representation limitations of Quaternions
|
|
float deltaAngle = Angle.Normalize(deltaRotation.eulerAngles.x);
|
|
this.rotationSpeed = deltaAngle / Time.deltaTime;
|
|
}
|
|
|
|
IEnumerator UpdateCore() {
|
|
WaitForSeconds wait = new(0.1f);
|
|
|
|
while (true) {
|
|
this.coreEncoder.angularSpeed = this.rotationSpeed;
|
|
yield return wait;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
#endif |