RoboidControl/RoboidControl-csharp
RoboidControl/RoboidControl-csharp
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases Activity

Compare commits

base: RoboidControl:c4c01bedae43947c00617fdcdcb50b38ebf32ff5
Branches Tags
RoboidControl:main
RoboidControl:v0.4_dev
RoboidControl:v0.3.0
..
compare: RoboidControl:71939b9aa52b62efca5191c01f9b940b6e845fcd
Branches Tags
RoboidControl:v0.4_dev
RoboidControl:main
RoboidControl:v0.3.0

No commits in common. "c4c01bedae43947c00617fdcdcb50b38ebf32ff5" and "71939b9aa52b62efca5191c01f9b940b6e845fcd" have entirely different histories.
c4c01bedae ... 71939b9aa5

15 changed files with 210 additions and 641 deletions
Show Stats Expand all files Collapse all files

3
Examples/BB2B/BB2B.cs
View File

@ -11,8 +11,9 @@ namespace RoboidControl {
public BB2B(Participant owner) : base(owner) { public BB2B(Participant owner) : base(owner) {
this.name = "BB2B"; this.name = "BB2B";
this.SetMotors(new Motor(this), new Motor(this));
this.SetDriveDimensions(0.064f, 0.128f); this.SetDriveDimensions(0.064f, 0.128f);
// this.wheelRadius = 0.032f;
// this.wheelSeparation = 0.128f;
// Is has a touch sensor at the front left of the roboid // Is has a touch sensor at the front left of the roboid
touchLeft = new(this) { touchLeft = new(this) {

55
Examples/BB2B/BB2B_Encoder.cs
View File

@ -1,55 +0,0 @@
using LinearAlgebra;
namespace RoboidControl {
// The robot is based on a differential drive
public class BB2B_Encoder : DifferentialDrive {
readonly DifferentialDrive drive;
readonly TouchSensor touchLeft;
readonly TouchSensor touchRight;
const float speed = 180.0f; // wheel rotation speed in degrees
public BB2B_Encoder(Participant owner) : base(owner) {
this.name = "BB2B";
this.SetDriveDimensions(0.064f, 0.128f);
// Update the basic motors to motors with encoder
EncoderMotor leftMotor = new(this, new RelativeEncoder()) {
position = new Spherical(0.064f, Direction.left)
};
EncoderMotor rightMotor = new(this, new RelativeEncoder()) {
position = new Spherical(0.064f, Direction.right)
};
this.SetMotors(leftMotor, rightMotor);
// Is has a touch sensor at the front left of the roboid
touchLeft = new(this) {
position = Spherical.Degrees(0.12f, -30, 0),
orientation = new SwingTwist(-30, 0, 0)
};
touchRight = new(this) {
position = Spherical.Degrees(0.12f, 30, 0),
orientation = new SwingTwist(30, 0, 0)
};
}
public override void Update(ulong currentTimeMs, bool recurse = true) {
// The left wheel turns forward when nothing is touched on the right side
// and turn backward when the roboid hits something on the right
float leftWheelVelocity = touchRight.touchedSomething ? -speed : speed;
// The right wheel does the same, but instead is controlled by
// touches on the left side
float rightWheelVelocity = touchLeft.touchedSomething ? -speed : speed;
// When both sides are touching something, both wheels will turn backward
// and the roboid will move backwards
this.SetWheelAngularVelocity(leftWheelVelocity, rightWheelVelocity);
base.Update(currentTimeMs, recurse);
}
}
}

118
Unity/DifferentialDrive.cs
View File

@ -2,110 +2,71 @@
using UnityEngine; using UnityEngine;
namespace RoboidControl.Unity { namespace RoboidControl.Unity {
/// <summary>
/// The Unity representation of a Roboid Control differential drive
/// </summary>
public class DifferentialDrive : Thing { public class DifferentialDrive : Thing {
/// <summary> public Wheel leftWheel;
/// The core differential drive public Wheel rightWheel;
/// </summary> public SphereCollider casterWheel;
public RoboidControl.DifferentialDrive coreDrive => core as RoboidControl.DifferentialDrive;
protected RoboidControl.DifferentialDrive coreDrive => core as RoboidControl.DifferentialDrive;
/// <summary> /// <summary>
/// Create the Unity representation /// Create the Unity representation
/// </summary> /// </summary>
/// <param name="coreDrive">The core touch sensor</param> /// <param name="core">The core touch sensor</param>
/// <returns>The Unity representation of the touch sensor</returns> /// <returns>The Unity representation of the touch sensor</returns>
/// This uses a 'DifferentialDrive' resource when available for the Unity representation. public static DifferentialDrive Create(RoboidControl.DifferentialDrive core) {
/// If this is not available, a default representation is created. DifferentialDrive component = null;
public static DifferentialDrive Create(RoboidControl.DifferentialDrive coreDrive) { Rigidbody rb = null;
DifferentialDrive differentialDrive;
GameObject prefab = (GameObject)Resources.Load("DifferentialDrive"); GameObject prefab = (GameObject)Resources.Load("DifferentialDrive");
if (prefab != null) { if (prefab != null) {
// Use resource prefab when available // Use resource prefab when available
GameObject gameObj = Instantiate(prefab); GameObject gameObj = Instantiate(prefab);
differentialDrive = gameObj.GetComponent<DifferentialDrive>(); component = gameObj.GetComponent<DifferentialDrive>();
differentialDrive.Init(coreDrive); if (component != null)
component.core = core;
rb = component.GetComponent<Rigidbody>();
} }
else { else {
// Default implementation // Fallback implementation
GameObject gameObj = new(coreDrive.name); GameObject gameObj = new(core.name);
differentialDrive = gameObj.AddComponent<DifferentialDrive>(); component = gameObj.AddComponent<DifferentialDrive>();
differentialDrive.Init(coreDrive); component.Init(core);
Rigidbody rb = gameObj.AddComponent<Rigidbody>(); rb = gameObj.AddComponent<Rigidbody>();
rb.isKinematic = false; rb.isKinematic = false;
rb.mass = 0.1f; rb.mass = 0.1f;
} }
return differentialDrive; return component;
} }
/// <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; private Rigidbody rb = null;
/// <summary>
/// Start the Unity representation
/// </summary>
protected virtual void Start() { protected virtual void Start() {
rb = GetComponent<Rigidbody>(); rb = GetComponent<Rigidbody>();
} }
/// <summary>
/// Handle the binary event indicating a configuration change
/// </summary>
protected override void HandleBinary() { protected override void HandleBinary() {
// Ignore it when the wheel radius is not set Debug.Log("Diff drive handle Binary");
if (coreDrive.wheelRadius <= 0) if (coreDrive.wheelRadius <= 0 || coreDrive.wheelSeparation <= 0)
return; return;
// Destroy any (generic) thing with the same id if (leftWheel == null)
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); 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 leftWheel.core ??= coreDrive.leftWheel;
if (rightWheel == null) { SphereCollider leftWheelCollider = leftWheel.GetComponent<SphereCollider>();
Thing[] things = FindObjectsOfType<Thing>(); leftWheelCollider.radius = coreDrive.wheelRadius;
foreach (Thing thing in things) { // leftWheelCollider.center = new Vector3(-coreDrive.wheelSeparation / 2, 0, 0);
if (thing.core.id == coreDrive.rightWheel.id)
Destroy(thing.gameObject); if (rightWheel == null)
}
}
if (rightWheel == null && coreDrive.rightWheel != null)
rightWheel = Wheel.Create(this.transform, coreDrive.rightWheel.id); rightWheel = Wheel.Create(this.transform, coreDrive.rightWheel.id);
if (rightWheel != null) {
rightWheel.core ??= coreDrive.rightWheel; rightWheel.core ??= coreDrive.rightWheel;
SphereCollider rightWheelCollider = rightWheel.GetComponent<SphereCollider>(); SphereCollider rightWheelCollider = rightWheel.GetComponent<SphereCollider>();
rightWheelCollider.radius = coreDrive.wheelRadius; rightWheelCollider.radius = coreDrive.wheelRadius;
} // rightWheelCollider.center = new Vector3(coreDrive.wheelSeparation / 2, 0, 0);
if (casterWheel == null) { if (casterWheel == null) {
GameObject gameObj = new("Caster wheel"); GameObject gameObj = new("Caster wheel");
@ -115,14 +76,9 @@ namespace RoboidControl.Unity {
} }
casterWheel.radius = coreDrive.wheelRadius; casterWheel.radius = coreDrive.wheelRadius;
// Put it in the middle of the back // 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... casterWheel.center = new Vector3(0, 0, -coreDrive.wheelSeparation);
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() { protected override void FixedUpdate() {
base.FixedUpdate(); base.FixedUpdate();
@ -130,10 +86,8 @@ namespace RoboidControl.Unity {
float leftWheelVelocity = leftWheel.rotationSpeed * (2 * Mathf.PI) * coreDrive.wheelRadius; float leftWheelVelocity = leftWheel.rotationSpeed * (2 * Mathf.PI) * coreDrive.wheelRadius;
float rightWheelVelocity = rightWheel.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 forwardSpeed = (leftWheelVelocity + rightWheelVelocity) / 2f;
float turningSpeed = (leftWheelVelocity - rightWheelVelocity) / wheelSeparation; float turningSpeed = (leftWheelVelocity - rightWheelVelocity) / coreDrive.wheelSeparation;
// Use smoothing to emulate motor inertia // Use smoothing to emulate motor inertia
rb.velocity = 0.9f * rb.velocity + 0.1f * forwardSpeed * transform.forward; rb.velocity = 0.9f * rb.velocity + 0.1f * forwardSpeed * transform.forward;

43
Unity/DistanceSensor.cs
View File

@ -5,45 +5,42 @@ using UnityEngine;
namespace RoboidControl.Unity { namespace RoboidControl.Unity {
/// <summary> /// <summary>
/// The Unity representation of a Roboid Control distance sensor /// The Unity representation of a distance sensor
/// </summary> /// </summary>
public class DistanceSensor : Thing { public class DistanceSensor : Thing {
/// <summary> /// <summary>
/// The core distance sensor /// The core distance sensor
/// </summary> /// </summary>
public RoboidControl.DistanceSensor coreSensor => base.core as RoboidControl.DistanceSensor; public new RoboidControl.DistanceSensor core {
get => (RoboidControl.DistanceSensor)base.core;
set => base.core = value;
}
/// <summary> /// <summary>
/// Start the Unity representation /// Start the Unity representation
/// </summary> /// </summary>
protected virtual void Start() { protected virtual void Start() {
if (core == null) {
SiteServer siteServer = FindAnyObjectByType<SiteServer>();
SetCoreThing(new RoboidControl.DistanceSensor(siteServer.site));
}
StartCoroutine(MeasureDistance()); StartCoroutine(MeasureDistance());
} }
/// <summary> /// <summary>
/// Create the Unity representation of the distance sensor /// Create the Unity representation of the distance sensor
/// </summary> /// </summary>
/// <param name="core">The core distance sensor</param> /// <param name="parent">The parent of the core distance sensor</param>
/// <returns>The Unity representation of the distance sensor</returns> /// <returns>The Unity representation of the distance sensor</returns>
/// This uses a 'DistanceSensor' resource when available for the Unity representation. public static DistanceSensor Create(RoboidControl.DistanceSensor core) {
/// If this is not available, a default representation is created. GameObject distanceObj = new("Distance sensor");
public static DistanceSensor Create(RoboidControl.DistanceSensor coreSensor) { DistanceSensor component = distanceObj.AddComponent<DistanceSensor>();
DistanceSensor distanceSensor; if (core.parent != null && core.parent.component != null)
GameObject prefab = (GameObject)Resources.Load("DistanceSensor"); distanceObj.transform.SetParent(core.parent.component.transform, false);
if (prefab != null) {
// Use resource prefab when available return component;
GameObject gameObj = Instantiate(prefab);
distanceSensor = gameObj.GetComponent<DistanceSensor>();
distanceSensor.Init(coreSensor);
}
else {
// Default implementation
GameObject distanceObj = new(coreSensor.name);
distanceSensor = distanceObj.AddComponent<DistanceSensor>();
distanceSensor.Init(coreSensor);
}
return distanceSensor;
} }
/// <summary> /// <summary>
@ -56,10 +53,10 @@ namespace RoboidControl.Unity {
Thing thing = hitInfo.transform.GetComponentInParent<Thing>(); Thing thing = hitInfo.transform.GetComponentInParent<Thing>();
if (thing == null) { if (thing == null) {
// Debug.Log($"collision {hitInfo.transform.name} {hitInfo.distance}"); // Debug.Log($"collision {hitInfo.transform.name} {hitInfo.distance}");
coreSensor.distance = hitInfo.distance; core.distance = hitInfo.distance;
} }
else else
coreSensor.distance = 0; core.distance = 0;
} }
yield return new WaitForSeconds(0.1f); yield return new WaitForSeconds(0.1f);
} }

67
Unity/Motor.cs
View File

@ -2,73 +2,50 @@
using UnityEngine; using UnityEngine;
namespace RoboidControl.Unity { namespace RoboidControl.Unity {
/// <summary>
/// The Unity representation of a Roboid Control motor
/// </summary>
public class Motor : Thing { public class Motor : Thing {
public float maxSpeed = 5;
/// <summary> /// <summary>
/// The core motor /// Create the Unity representation
/// </summary> /// </summary>
public RoboidControl.Motor coreMotor => base.core as RoboidControl.Motor; /// <param name="core">The core motor</param>
/// <summary>
/// Create the Unity representation of the motor
/// </summary>
/// <param name="coreMotor">The core motor</param>
/// <returns>The Unity representation of a motor</returns> /// <returns>The Unity representation of a motor</returns>
/// This uses a 'Motor' resource when available for the Unity representation. public static Motor Create(RoboidControl.Motor core) {
/// If this is not available, a default representation is created.
public static Motor Create(RoboidControl.Motor coreMotor) {
Motor motor;
GameObject prefab = (GameObject)Resources.Load("Motor"); GameObject prefab = (GameObject)Resources.Load("Motor");
if (prefab != null) { if (prefab != null) {
// Use resource prefab when available // Use resource prefab when available
GameObject gameObj = Instantiate(prefab); GameObject gameObj = Instantiate(prefab);
motor = gameObj.GetComponent<Motor>(); Motor component = gameObj.GetComponent<Motor>();
motor.Init(coreMotor); if (component != null)
component.core = core;
return component;
} }
else { else {
// Default implementation // Fallback implementation
GameObject gameObj = new(coreMotor.name); GameObject gameObj = new(core.name);
motor = gameObj.AddComponent<Motor>(); Motor component = gameObj.AddComponent<Motor>();
motor.Init(coreMotor); component.Init(core);
Rigidbody rb = gameObj.AddComponent<Rigidbody>(); Rigidbody rb = gameObj.AddComponent<Rigidbody>();
rb.isKinematic = true; rb.isKinematic = true;
return component;
} }
return motor;
} }
/// <summary> public float rotationSpeed = 0.0f;
/// The maximum rotation speed of the motor in rotations per second
/// </summary>
public float maxSpeed = 5;
/// <summary>
/// The actual rotation speed in rotations per second
/// </summary>
public float rotationSpeed { get; protected set; }
/// <summary>
/// Update the Unity state
/// </summary>
protected override void FixedUpdate() {
base.FixedUpdate();
// We rotate the first child of the motor, which should be the axle.
float rotation = 360 * this.rotationSpeed * Time.fixedDeltaTime;
if (this.transform.childCount > 0)
this.transform.GetChild(0).Rotate(rotation, 0, 0);
}
/// <summary>
/// Handle the binary event containing the rotation speed
/// </summary>
protected override void HandleBinary() { protected override void HandleBinary() {
RoboidControl.Motor coreMotor = core as RoboidControl.Motor; RoboidControl.Motor coreMotor = core as RoboidControl.Motor;
this.rotationSpeed = coreMotor.targetSpeed * maxSpeed; this.rotationSpeed = coreMotor.targetSpeed * maxSpeed;
} }
protected override void Update() {
base.Update();
// We rotate the first child of the motor, which should be the axle.
float rotation = 360 * this.rotationSpeed * Time.deltaTime;
if (this.transform.childCount > 0)
this.transform.GetChild(0).Rotate(rotation, 0, 0);
}
} }
} }
#endif #endif

3
Unity/Participant.cs
View File

@ -27,12 +27,10 @@ namespace RoboidControl.Unity {
protected virtual void HandleThingEvent(RoboidControl.Participant.UpdateEvent e) { protected virtual void HandleThingEvent(RoboidControl.Participant.UpdateEvent e) {
switch (e.thing) { switch (e.thing) {
case RoboidControl.TouchSensor coreTouchSensor: case RoboidControl.TouchSensor coreTouchSensor:
Debug.Log("Handle TouchSensor");
TouchSensor touchSensor = TouchSensor.Create(coreTouchSensor); TouchSensor touchSensor = TouchSensor.Create(coreTouchSensor);
coreTouchSensor.component = touchSensor; coreTouchSensor.component = touchSensor;
break; break;
case RoboidControl.DifferentialDrive coreDrive: case RoboidControl.DifferentialDrive coreDrive:
Debug.Log("Handle Diff.Drive");
DifferentialDrive differentialDrive = DifferentialDrive.Create(coreDrive); DifferentialDrive differentialDrive = DifferentialDrive.Create(coreDrive);
coreDrive.component = differentialDrive; coreDrive.component = differentialDrive;
break; break;
@ -43,7 +41,6 @@ namespace RoboidControl.Unity {
// // before we can create the wheel reliably // // before we can create the wheel reliably
// break; // break;
case RoboidControl.Thing coreThing: case RoboidControl.Thing coreThing:
Debug.Log("Handle Thing");
if (coreThing.component == null) { if (coreThing.component == null) {
Thing thing = Thing.Create(coreThing); Thing thing = Thing.Create(coreThing);
coreThing.component = thing; coreThing.component = thing;

73
Unity/RelativeEncoder.cs
View File

@ -1,73 +0,0 @@
#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

163
Unity/Thing.cs
View File

@ -2,32 +2,39 @@
using System.Collections; using System.Collections;
using UnityEngine; using UnityEngine;
using UnityEngine.Networking; using UnityEngine.Networking;
#if GLTF
using GLTFast;
#endif
namespace RoboidControl.Unity { namespace RoboidControl.Unity {
/// <summary> /// <summary>
/// The Unity representation fo a Roboid Control Thing /// The representation of a Thing in Unity
/// </summary> /// </summary>
public class Thing : MonoBehaviour { public class Thing : MonoBehaviour {
/// <summary> /// <summary>
/// The core C# thing /// The core C# thing
/// </summary> /// </summary>
//[field: SerializeField]
public RoboidControl.Thing core { get; set; } public RoboidControl.Thing core { get; set; }
/// <summary> public SiteServer participant;
/// The owner of this thing
/// </summary> private string modelUrl = null;
public Participant owner;
/// <summary> /// <summary>
/// Create a Unity representation of a Thing /// Set the core C# thing
/// </summary> /// </summary>
/// <param name="core">The core of the thing</param> protected void SetCoreThing(RoboidControl.Thing thing) {
/// <returns>The created thing</returns> core = thing;
core.component = this;
SiteServer siteServer = FindAnyObjectByType<SiteServer>();
if (siteServer == null || siteServer.site == null) {
Debug.LogWarning("No site server found");
return;
}
siteServer.site.Add(thing);
}
public static Thing Create(RoboidControl.Thing core) { public static Thing Create(RoboidControl.Thing core) {
// Debug.Log("Creating new Unity thing"); // Debug.Log("Creating new Unity thing");
GameObject gameObj = string.IsNullOrEmpty(core.name) ? GameObject gameObj = string.IsNullOrEmpty(core.name) ?
@ -38,17 +45,11 @@ namespace RoboidControl.Unity {
return component; return component;
} }
/// <summary>
/// Initialize the Thing
/// </summary>
/// <param name="core">The core of the thing</param>
/// This affects the parent and pose of the thing
protected void Init(RoboidControl.Thing core) { protected void Init(RoboidControl.Thing core) {
this.core = core; this.core = core;
this.core.component = this; this.core.component = this;
this.owner = FindAnyObjectByType<SiteServer>(); this.participant = FindAnyObjectByType<SiteServer>();
core.owner = this.owner.coreParticipant; core.owner = this.participant.coreParticipant;
if (core.parent != null && core.parent.component != null) { if (core.parent != null && core.parent.component != null) {
this.transform.SetParent(core.parent.component.transform, false); this.transform.SetParent(core.parent.component.transform, false);
this.transform.localPosition = Vector3.zero; this.transform.localPosition = Vector3.zero;
@ -61,7 +62,7 @@ namespace RoboidControl.Unity {
} }
/// <summary> /// <summary>
/// Update the Unity rendering /// Update the Unity representation
/// </summary> /// </summary>
protected virtual void Update() { protected virtual void Update() {
if (core == null) if (core == null)
@ -78,16 +79,10 @@ namespace RoboidControl.Unity {
} }
} }
/// <summary>
/// Update the Unity state (just calls UpdateThing)
/// </summary>
protected virtual void FixedUpdate() { protected virtual void FixedUpdate() {
UpdateThing(); UpdateThing();
} }
/// <summary>
/// Update the Unity state
/// </summary>
public void UpdateThing() { public void UpdateThing() {
if (core == null) { if (core == null) {
Debug.Log($"{this} core thing is gone, self destruct in 0 seconds..."); Debug.Log($"{this} core thing is gone, self destruct in 0 seconds...");
@ -95,50 +90,37 @@ namespace RoboidControl.Unity {
return; return;
} }
while (core.updateQueue.TryDequeue(out RoboidControl.Thing.CoreEvent e)) if (core.updateQueue.TryDequeue(out RoboidControl.Thing.UpdateEvent e))
HandleCoreEvent(e); HandleUpdateEvent(e);
} }
/// <summary> private void HandleUpdateEvent(RoboidControl.Thing.UpdateEvent e) {
/// Handle events from the core thing switch (e.messageId) {
/// </summary>
/// <param name="coreEvent">The core event to handle</param>
private void HandleCoreEvent(RoboidControl.Thing.CoreEvent coreEvent) {
switch (coreEvent.messageId) {
case ThingMsg.id: case ThingMsg.id:
Debug.Log($"{this.core.id} Handle Thing");
if (core.parent == null) if (core.parent == null)
this.transform.SetParent(null, true); this.transform.SetParent(null, true);
else if (core.parent.component != null) else if (core.parent.component != null)
this.transform.SetParent(core.parent.component.transform, true); this.transform.SetParent(core.parent.component.transform, true);
break; break;
case NameMsg.Id: case NameMsg.Id:
Debug.Log($"{this.core.id} Handle Name");
this.gameObject.name = core.name; this.gameObject.name = core.name;
break; break;
case ModelUrlMsg.Id: case ModelUrlMsg.Id:
Debug.Log("{this.id} Handle Model URL");
string extension = core.modelUrl[core.modelUrl.LastIndexOf(".")..]; string extension = core.modelUrl[core.modelUrl.LastIndexOf(".")..];
if (extension == ".jpg" || extension == ".png") if (extension == ".jpg" || extension == ".png")
StartCoroutine(LoadJPG()); StartCoroutine(LoadJPG());
else if (extension == ".gltf" || extension == ".glb")
ProcessGltfModel(core); this.modelUrl = core.modelUrl;
break; break;
case PoseMsg.Id: case PoseMsg.Id:
Debug.Log($"{this.core.id} Handle Pose");
this.HandlePose(); this.HandlePose();
break; break;
case BinaryMsg.Id: case BinaryMsg.Id:
Debug.Log($"{this.core.id} Handle Binary");
this.HandleBinary(); this.HandleBinary();
break; break;
} }
} }
/// <summary>
/// Load and attach a JPG sprite visualization of the thing
/// </summary>
/// <returns></returns>
private IEnumerator LoadJPG() { private IEnumerator LoadJPG() {
UnityWebRequest request = UnityWebRequestTexture.GetTexture(core.modelUrl); UnityWebRequest request = UnityWebRequestTexture.GetTexture(core.modelUrl);
yield return request.SendWebRequest(); yield return request.SendWebRequest();
@ -166,93 +148,6 @@ namespace RoboidControl.Unity {
} }
} }
bool loadingModel = false;
private async void ProcessGltfModel(RoboidControl.Thing coreThing) {
#if GLTF
if (!loadingModel) {
loadingModel = true;
Debug.Log("Loading GLTF model from :" + coreThing.modelUrl);
GltfImport gltfImport = new GltfImport();
bool success = await gltfImport.Load(coreThing.modelUrl);
if (success) {
Transform parentTransform = this.transform;
Thing[] things = FindObjectsOfType<Thing>();
Thing parentThing = null;
foreach (Thing thing in things) {
if (thing.core.id == coreThing.id) {
parentTransform = thing.transform;
parentThing = thing;
}
}
await gltfImport.InstantiateMainSceneAsync(parentTransform);
SkinnedMeshRenderer[] meshRenderers = parentTransform.GetComponentsInChildren<SkinnedMeshRenderer>();
#if pHUMANOID4
if (parentThing.objectType == 7) {
HumanoidControl hc = parentThing.gameObject.GetComponent<HumanoidControl>();
if (hc == null)
hc = parentThing.gameObject.AddComponent<HumanoidControl>();
foreach (SkinnedMeshRenderer meshRenderer in meshRenderers) {
if (meshRenderer.rootBone != null) {
Debug.Log("Found a skinned mesh with bones");
hc.RetrieveBonesFrom(meshRenderer.rootBone);
break;
}
}
}
#endif
parentTransform.localScale = Vector3.one;
if (meshRenderers.Length > 0) {
foreach (SkinnedMeshRenderer meshRenderer in meshRenderers) {
if (meshRenderer.rootBone != null) {
Debug.Log("Found a skinned mesh with bones");
ScanForThings(meshRenderer.rootBone);
break;
}
}
}
else {
ScanForThings(parentTransform);
}
}
else {
this.transform.localScale = Vector3.one * 1;
}
}
loadingModel = true;
#endif
}
private void ScanForThings(Transform rootTransform) {
// Thing[] thingArray = allThings.ToArray();
// for (int thingIx = 0; thingIx < thingArray.Length; thingIx++) {
// Thing thing = thingArray[thingIx];
// GameObject foundObj = FindThingByName(thing, rootTransform);
// if (foundObj != null && foundObj != thing.gameObject) {
// Thing foundThing = foundObj.GetComponent<Thing>();
// if (foundThing == null) {
// allThings.Remove(thing);
// foundThing = foundObj.AddComponent<Thing>();
// foundThing.networkId = thing.networkId;
// foundThing.objectId = thing.objectId;
// allThings.Add(foundThing);
// Destroy(thing.gameObject);
// }
// }
// }
}
/// <summary>
/// Handle a Pose event
/// </summary>
/// This can update the position and/or orientation when the velocity of the thing is zero.
/// If a velocity is not zero, the position and/or orientation update will be ignored
protected virtual void HandlePose() { protected virtual void HandlePose() {
if (core.linearVelocity.distance == 0) if (core.linearVelocity.distance == 0)
this.transform.localPosition = core.position.ToVector3(); this.transform.localPosition = core.position.ToVector3();
@ -260,10 +155,6 @@ namespace RoboidControl.Unity {
this.transform.localRotation = core.orientation.ToQuaternion(); this.transform.localRotation = core.orientation.ToQuaternion();
} }
/// <summary>
/// Handle a Binary event
/// </summary>
protected virtual void HandleBinary() { } protected virtual void HandleBinary() { }
} }

75
Unity/TouchSensor.cs
View File

@ -4,36 +4,52 @@ using UnityEngine;
namespace RoboidControl.Unity { namespace RoboidControl.Unity {
/// <summary> /// <summary>
/// The Unity representation of a Roboid Control touch sensor /// The Unity representation of the TouchSensor
/// </summary> /// </summary>
public class TouchSensor : Thing { public class TouchSensor : Thing {
// public SiteServer participant;
/// <summary> /// <summary>
/// The core touch sensor /// The core touch sensor
/// </summary> /// </summary>
public RoboidControl.TouchSensor coreSensor => base.core as RoboidControl.TouchSensor; public RoboidControl.TouchSensor coreSensor {
get => base.core as RoboidControl.TouchSensor;
set => base.core = value;
}
SphereCollider touchCollider = null;
/// <summary> /// <summary>
/// Create the Unity representation of the touch sensor /// Start the Unity represention
/// </summary> /// </summary>
/// <param name="coreSensor">The core touch sensor</param> protected virtual void Start() {
if (core == null) {
participant = FindAnyObjectByType<SiteServer>();
SetCoreThing(new RoboidControl.TouchSensor(participant.site));
}
touchCollider = GetComponent<SphereCollider>();
}
/// <summary>
/// Create the Unity representation
/// </summary>
/// <param name="core">The core touch sensor</param>
/// <returns>The Unity representation of the touch sensor</returns> /// <returns>The Unity representation of the touch sensor</returns>
/// This uses a 'TouchSensor' resource when available for the Unity representation. public static TouchSensor Create(RoboidControl.TouchSensor core) {
/// If this is not available, a default representation is created.
public static TouchSensor Create(RoboidControl.TouchSensor coreSensor) {
TouchSensor touchSensor;
GameObject prefab = (GameObject)Resources.Load("TouchSensor"); GameObject prefab = (GameObject)Resources.Load("TouchSensor");
if (prefab != null) { if (prefab != null) {
// Use resource prefab when available // Use resource prefab when available
GameObject gameObj = Instantiate(prefab); GameObject gameObj = Instantiate(prefab);
touchSensor = gameObj.GetComponent<TouchSensor>(); TouchSensor component = gameObj.GetComponent<TouchSensor>();
touchSensor.Init(coreSensor); if (component != null)
component.core = core;
return component;
} }
else { else {
// Default implementation // Fallback implementation
GameObject gameObj = new(coreSensor.name); GameObject gameObj = new(core.name);
touchSensor = gameObj.AddComponent<TouchSensor>(); TouchSensor component = gameObj.AddComponent<TouchSensor>();
touchSensor.Init(coreSensor); component.Init(core);
Rigidbody rb = gameObj.AddComponent<Rigidbody>(); Rigidbody rb = gameObj.AddComponent<Rigidbody>();
rb.isKinematic = true; rb.isKinematic = true;
@ -41,33 +57,30 @@ namespace RoboidControl.Unity {
SphereCollider collider = gameObj.AddComponent<SphereCollider>(); SphereCollider collider = gameObj.AddComponent<SphereCollider>();
collider.radius = 0.01f; collider.radius = 0.01f;
collider.isTrigger = true; collider.isTrigger = true;
return component;
} }
return touchSensor;
} }
/// <summary>
/// Handle touch trigger collider enter event
/// </summary>
/// <param name="other">The collider which entered our trigger collider</param>
private void OnTriggerEnter(Collider other) { private void OnTriggerEnter(Collider other) {
// Don't detect trigger colliders // Debug.Log("Touch?");
if (other.isTrigger) if (other.isTrigger) {
return; // Debug.Log($" was trigger {other.name}");
// Don't touch yourself return;
if (this.transform.root == other.transform.root) }
return; if (this.transform.root == other.transform.root) {
Debug.Log($" was myself {other.name}");
return;
}
this.coreSensor.touchedSomething = true; Debug.Log($"*** {this} Touch");
this.core.updateQueue.Enqueue(new RoboidControl.Thing.CoreEvent(BinaryMsg.Id)); this.coreSensor.touchedSomething = true;
this.core.updateQueue.Enqueue(new RoboidControl.Thing.UpdateEvent(BinaryMsg.Id));
} }
/// <summary>
/// Handl touch trigger collider exit event
/// </summary>
/// <param name="other">The collider which exited our trigger collider </param>
private void OnTriggerExit(Collider other) { private void OnTriggerExit(Collider other) {
if (other.isTrigger) if (other.isTrigger)
return; return;
Debug.Log($"*** {this} Touch end");
this.coreSensor.touchedSomething = false; this.coreSensor.touchedSomething = false;
} }
} }

20
Unity/Wheel.cs
View File

@ -2,10 +2,6 @@
using UnityEngine; using UnityEngine;
namespace RoboidControl.Unity { namespace RoboidControl.Unity {
/// <summary>
/// The Unity representation of a Roboid Control wheel
/// </summary>
public class Wheel : Motor { public class Wheel : Motor {
/// <summary> /// <summary>
/// Create the Unity representation /// Create the Unity representation
@ -27,6 +23,14 @@ namespace RoboidControl.Unity {
GameObject gameObj = new(core.name); GameObject gameObj = new(core.name);
Wheel component = gameObj.AddComponent<Wheel>(); Wheel component = gameObj.AddComponent<Wheel>();
component.Init(core); component.Init(core);
// component.wheelCollider = gameObj.AddComponent<WheelCollider>();
// component.wheelCollider.mass = 0.1f;
// component.wheelCollider.suspensionDistance = 0.01f;
// component.wheelCollider.suspensionSpring = new JointSpring {
// spring = 100f, // Very high spring value to make it rigid
// damper = 10f, // Low damping (could be adjusted for slight 'bounciness')
// targetPosition = 0.5f // Neutral position (middle of the suspension travel)
// };
Debug.Log("Create " + core.name); Debug.Log("Create " + core.name);
return component; return component;
} }
@ -57,6 +61,14 @@ namespace RoboidControl.Unity {
SphereCollider collider = gameObj.AddComponent<SphereCollider>(); SphereCollider collider = gameObj.AddComponent<SphereCollider>();
collider.radius = 0.00001f; collider.radius = 0.00001f;
collider.material = slidingWheel; collider.material = slidingWheel;
// component.wheelCollider = gameObj.AddComponent<WheelCollider>();
// component.wheelCollider.mass = 0.1f;
// component.wheelCollider.suspensionDistance = 0.01f;
// component.wheelCollider.suspensionSpring = new JointSpring {
// spring = 100f, // Very high spring value to make it rigid
// damper = 10f, // Low damping (could be adjusted for slight 'bounciness')
// targetPosition = 0.5f // Neutral position (middle of the suspension travel)
// };
Debug.Log("Create placeholder Wheel "); Debug.Log("Create placeholder Wheel ");
return component; return component;
} }

19
src/Thing.cs
View File

@ -26,7 +26,6 @@ namespace RoboidControl {
public const byte ControlledMotor = 0x06; public const byte ControlledMotor = 0x06;
public const byte UncontrolledMotor = 0x07; public const byte UncontrolledMotor = 0x07;
public const byte Servo = 0x08; public const byte Servo = 0x08;
public const byte IncrementalEncoder = 0x19;
// Other // Other
public const byte Roboid = 0x09; public const byte Roboid = 0x09;
public const byte HUmanoid = 0x0A; public const byte HUmanoid = 0x0A;
@ -138,7 +137,7 @@ namespace RoboidControl {
if (_name != value) { if (_name != value) {
_name = value; _name = value;
nameChanged = true; nameChanged = true;
this.updateQueue.Enqueue(new CoreEvent(NameMsg.Id)); this.updateQueue.Enqueue(new UpdateEvent(NameMsg.Id));
} }
} }
} }
@ -153,7 +152,7 @@ namespace RoboidControl {
set { set {
if (_modelUrl != value) { if (_modelUrl != value) {
_modelUrl = value; _modelUrl = value;
this.updateQueue.Enqueue(new CoreEvent(ModelUrlMsg.Id)); this.updateQueue.Enqueue(new UpdateEvent(ModelUrlMsg.Id));
} }
} }
} }
@ -188,7 +187,7 @@ namespace RoboidControl {
value.AddChild(this); value.AddChild(this);
} }
this.hierarchyChanged = true; this.hierarchyChanged = true;
this.updateQueue.Enqueue(new CoreEvent(ThingMsg.id)); this.updateQueue.Enqueue(new UpdateEvent(ThingMsg.id));
} }
} }
@ -283,7 +282,7 @@ namespace RoboidControl {
if (_position != value) { if (_position != value) {
_position = value; _position = value;
positionUpdated = true; positionUpdated = true;
updateQueue.Enqueue(new CoreEvent(PoseMsg.Id)); updateQueue.Enqueue(new UpdateEvent(PoseMsg.Id));
} }
} }
} }
@ -321,7 +320,7 @@ namespace RoboidControl {
if (_linearVelocity != value) { if (_linearVelocity != value) {
_linearVelocity = value; _linearVelocity = value;
linearVelocityUpdated = true; linearVelocityUpdated = true;
updateQueue.Enqueue(new CoreEvent(PoseMsg.Id)); updateQueue.Enqueue(new UpdateEvent(PoseMsg.Id));
} }
} }
} }
@ -340,7 +339,7 @@ namespace RoboidControl {
if (_angularVelocity != value) { if (_angularVelocity != value) {
_angularVelocity = value; _angularVelocity = value;
angularVelocityUpdated = true; angularVelocityUpdated = true;
updateQueue.Enqueue(new CoreEvent(PoseMsg.Id)); updateQueue.Enqueue(new UpdateEvent(PoseMsg.Id));
} }
} }
} }
@ -396,13 +395,13 @@ namespace RoboidControl {
} }
} }
public class CoreEvent { public class UpdateEvent {
public CoreEvent(int messageId) { public UpdateEvent(int messageId) {
this.messageId = messageId; this.messageId = messageId;
} }
public int messageId; // see the communication messages public int messageId; // see the communication messages
}; };
public ConcurrentQueue<CoreEvent> updateQueue = new(); public ConcurrentQueue<UpdateEvent> updateQueue = new();
#endregion Update #endregion Update

82
src/Things/DifferentialDrive.cs
View File

@ -18,16 +18,16 @@ namespace RoboidControl {
/// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param> /// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
/// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param> /// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
public DifferentialDrive(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.DifferentialDrive, thingId, invokeEvent) { public DifferentialDrive(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.DifferentialDrive, thingId, invokeEvent) {
// Motor leftWheel = new(this) { Motor leftWheel = new(this) {
// name = "Left Wheel" name = "Left Wheel"
// }; };
// Motor rightWheel = new(this) { Motor rightWheel = new(this) {
// name = "Right Wheel" name = "Right Wheel"
// }; };
// SetMotors(leftWheel, rightWheel); SetMotors(leftWheel, rightWheel);
// sendBinary = true; sendBinary = true;
// owner.Send(new BinaryMsg(owner.networkId, this)); owner.Send(new BinaryMsg(owner.networkId, this));
// this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id)); this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id));
} }
/// <summary> /// <summary>
@ -45,38 +45,31 @@ namespace RoboidControl {
/// These values are used to compute the desired wheel speed from the set /// These values are used to compute the desired wheel speed from the set
/// linear and angular velocity. /// linear and angular velocity.
/// @sa SetLinearVelocity SetAngularVelocity /// @sa SetLinearVelocity SetAngularVelocity
public void SetDriveDimensions(float wheelDiameter, float wheelSeparation = 0) { public void SetDriveDimensions(float wheelDiameter, float wheelSeparation) {
this._wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2; this._wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
this._wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
this.rpsToMs = wheelDiameter * Angle.pi; this.rpsToMs = wheelDiameter * Angle.pi;
if (wheelSeparation > 0) { float distance = this.wheelSeparation / 2;
wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation; if (this.leftWheel != null)
float distance = wheelSeparation / 2; this.leftWheel.position = new Spherical(distance, Direction.left);
if (this.leftWheel != null) if (this.rightWheel != null)
this.leftWheel.position = new Spherical(distance, Direction.left); this.rightWheel.position = new Spherical(distance, Direction.right);
if (this.rightWheel != null)
this.rightWheel.position = new Spherical(distance, Direction.right);
}
} }
/// @brief Congures the motors for the wheels /// @brief Congures the motors for the wheels
/// @param leftWheel The motor for the left wheel /// @param leftWheel The motor for the left wheel
/// @param rightWheel The motor for the right wheel /// @param rightWheel The motor for the right wheel
public void SetMotors(Motor leftWheel, Motor rightWheel) { public void SetMotors(Motor leftWheel, Motor rightWheel) {
// float distance = this.wheelSeparation / 2; float distance = this.wheelSeparation / 2;
this.leftWheel = leftWheel; this.leftWheel = leftWheel;
this.leftWheel.parent = this; if (this.leftWheel != null)
// if (this.leftWheel != null) this.leftWheel.position = new Spherical(distance, Direction.left);
// this.leftWheel.position = new Spherical(distance, Direction.left);
this.rightWheel = rightWheel; this.rightWheel = rightWheel;
this.rightWheel.parent= this; if (this.rightWheel != null)
// if (this.rightWheel != null) this.rightWheel.position = new Spherical(distance, Direction.right);
// this.rightWheel.position = new Spherical(distance, Direction.right);
owner.Send(new BinaryMsg(owner.networkId, this));
this.updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id));
} }
/// @brief Directly specify the speeds of the motors /// @brief Directly specify the speeds of the motors
@ -94,18 +87,6 @@ namespace RoboidControl {
//this.rightWheel.angularVelocity = new Spherical(speedRight, Direction.right); //this.rightWheel.angularVelocity = new Spherical(speedRight, Direction.right);
} }
} }
/// @brief Directly specify the speeds of the motors
/// @param speedLeft The speed of the left wheel in degrees per second.
/// Positive moves the robot in the forward direction.
/// @param speedRight The speed of the right wheel in degrees per second.
/// Positive moves the robot in the forward direction.
public void SetWheelAngularVelocity(float angularSpeedLeft, float angularSpeedRight) {
// This only works when the motor is a motor with encoder
if (this.leftWheel is EncoderMotor leftMotor)
leftMotor.targetAngularSpeed = angularSpeedLeft;
if (this.rightWheel is EncoderMotor rightMotor)
rightMotor.targetAngularSpeed = angularSpeedRight;
}
/// @copydoc RoboidControl::Thing::Update(unsigned long) /// @copydoc RoboidControl::Thing::Update(unsigned long)
public override void Update(ulong currentMs, bool recursive = true) { public override void Update(ulong currentMs, bool recursive = true) {
@ -119,10 +100,9 @@ namespace RoboidControl {
if (angularVelocity.direction.horizontal < 0) if (angularVelocity.direction.horizontal < 0)
angularSpeed = -angularSpeed; angularSpeed = -angularSpeed;
// This assumes that the left wheel position has Direction.left // wheel separation can be replaced by this.leftwheel.position.distance
// and the right wheel position has Direction.Right... float speedLeft = (linearVelocity + angularSpeed * this.wheelSeparation / 2) / this.wheelRadius * Angle.Rad2Deg;
float speedLeft = (linearVelocity + angularSpeed * this.leftWheel.position.distance) / this.wheelRadius * Angle.Rad2Deg; float speedRight = (linearVelocity - angularSpeed * this.wheelSeparation / 2) / this.wheelRadius * Angle.Rad2Deg;
float speedRight = (linearVelocity - angularSpeed * this.rightWheel.position.distance) / this.wheelRadius * Angle.Rad2Deg;
this.SetWheelVelocity(speedLeft, speedRight); this.SetWheelVelocity(speedLeft, speedRight);
} }
} }
@ -131,8 +111,8 @@ namespace RoboidControl {
private float _wheelRadius = 0.0f; private float _wheelRadius = 0.0f;
public float wheelRadius { get => _wheelRadius; } public float wheelRadius { get => _wheelRadius; }
/// @brief The distance between the wheels in meters /// @brief The distance between the wheels in meters
// private float _wheelSeparation = 0.0f; private float _wheelSeparation = 0.0f;
// public float wheelSeparation { get => _wheelSeparation; } public float wheelSeparation { get => _wheelSeparation; }
/// @brief Convert revolutions per second to meters per second /// @brief Convert revolutions per second to meters per second
protected float rpsToMs = 1.0f; protected float rpsToMs = 1.0f;
@ -147,12 +127,12 @@ namespace RoboidControl {
if (!sendBinary) if (!sendBinary)
return System.Array.Empty<byte>(); return System.Array.Empty<byte>();
byte[] data = new byte[4]; byte[] data = new byte[6];
byte ix = 0; byte ix = 0;
data[ix++] = leftWheel.id; data[ix++] = leftWheel.id;
data[ix++] = rightWheel.id; data[ix++] = rightWheel.id;
LowLevelMessages.SendFloat16(data, ref ix, wheelRadius); LowLevelMessages.SendFloat16(data, ref ix, wheelRadius);
//LowLevelMessages.SendFloat16(data, ref ix, wheelSeparation); LowLevelMessages.SendFloat16(data, ref ix, wheelSeparation);
sendBinary = false; sendBinary = false;
return data; return data;
} }
@ -164,8 +144,8 @@ namespace RoboidControl {
byte rightWheelId = data[ix++]; byte rightWheelId = data[ix++];
this.rightWheel = this.owner.Get(rightWheelId) as Motor; this.rightWheel = this.owner.Get(rightWheelId) as Motor;
this._wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix); this._wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix);
//this._wheelSeparation = LowLevelMessages.ReceiveFloat16(data, ref ix); this._wheelSeparation = LowLevelMessages.ReceiveFloat16(data, ref ix);
this.updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id)); this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id));
} }
}; };

84
src/Things/EncoderMotor.cs
View File

@ -1,84 +0,0 @@
namespace RoboidControl {
/// @brief A motor with speed control
/// It uses a feedback loop from an encoder to regulate the speed
/// The speed is measured in revolutions per second.
class EncoderMotor : Motor {
public EncoderMotor(Thing parent, RelativeEncoder encoder) : base(parent) {
this.encoder = encoder;
}
// Upgrade an existing motor with an encoder
public EncoderMotor(Motor motor, RelativeEncoder encoder) : base(motor.parent) {
this.motor = motor;
this.encoder = encoder;
}
readonly RelativeEncoder encoder;
readonly Motor motor;
public override float targetSpeed {
get {
if (this.motor != null)
return this.motor.targetSpeed;
else
return base.targetSpeed;
}
set {
if (this.motor != null)
this.motor.targetSpeed = value;
else
base.targetSpeed = value;
}
}
private float _targetAngularSpeed; // In degrees per second
public virtual float targetAngularSpeed {
get => _targetAngularSpeed;
set {
if (value != this._targetAngularSpeed) {
this._targetAngularSpeed = value;
updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id));
owner.Send(new BinaryMsg(this));
}
}
}
/// @brief Get the actual speed from the encoder
/// @return The speed in angle per second
public virtual float actualAngularSpee {
get => encoder.angularSpeed;
}
float pidP = 0.1F;
float pidD = 0.0F;
float pidI = 0.0F;
public override void Update(ulong currentTimeMs, bool recurse = false) {
float actualSpeed = this.encoder.angularSpeed;
// Simplified rotation direction, shouldbe improved
// This goes wrong when the target speed is inverted and the motor axcle is still turning in the old direction
if (this.targetAngularSpeed < 0)
actualSpeed = -actualSpeed;
float deltaTime = (currentTimeMs - this.lastUpdateTime) / 1000.0f;
float error = this.targetAngularSpeed - actualSpeed;
float errorChange = (error - lastError) * deltaTime;
float delta = error * pidP + errorChange * pidD;
// Update the motor speed
if (motor != null)
motor.targetSpeed += delta;
else
base.targetSpeed += delta;
this.lastUpdateTime = currentTimeMs;
}
ulong lastUpdateTime = 0;
float lastError = 0;
// enum Direction { Forward = 1, Reverse = -1 };
// Direction rotationDirection;
};
} // namespace RoboidControl

6
src/Things/Motor.cs
View File

@ -16,16 +16,16 @@ namespace RoboidControl {
protected float currentTargetSpeed = 0; protected float currentTargetSpeed = 0;
private float _targetSpeed = 0; private float _targetSpeed;
/// <summary> /// <summary>
/// The speed between -1 (full reverse), 0 (stop) and 1 (full forward) /// The speed between -1 (full reverse), 0 (stop) and 1 (full forward)
/// </summary> /// </summary>
public virtual float targetSpeed { public float targetSpeed {
get => _targetSpeed; get => _targetSpeed;
set { set {
if (value != _targetSpeed) { if (value != _targetSpeed) {
_targetSpeed = Float.Clamp(value, -1, 1); _targetSpeed = Float.Clamp(value, -1, 1);
updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id)); updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id));
owner.Send(new BinaryMsg(this)); owner.Send(new BinaryMsg(this));
} }
} }

40
src/Things/RelativeEncoder.cs
View File

@ -1,40 +0,0 @@
namespace RoboidControl {
/// @brief An Incremental Encoder measures the rotations of an axle using a rotary
/// sensor. Some encoders are able to detect direction, while others can not.
public class RelativeEncoder : Thing {
/// @brief Creates a sensor which measures distance from pulses
/// @param pulsesPerRevolution The number of pulse edges which make a
/// full rotation
/// @param distancePerRevolution The distance a wheel travels per full
/// rotation
public RelativeEncoder(bool invokeEvent = true) : base(Type.IncrementalEncoder, invokeEvent) { }
protected float _rotationSpeed = 0;
/// @brief Get the rotation speed since the previous call
/// @param currentTimeMs The clock time in milliseconds
/// @return The speed in rotations per second
public virtual float angularSpeed {
get => _rotationSpeed;
set {
if (_rotationSpeed != value) {
_rotationSpeed = value;
this.owner.Send(new BinaryMsg(this));
}
}
}
public override byte[] GenerateBinary() {
byte[] data = new byte[1];
byte ix = 0;
data[ix++] = (byte)(this._rotationSpeed * 127);
return data;
}
public override void ProcessBinary(byte[] data) {
this._rotationSpeed = (float)data[0] / 127;
}
};
} // namespace RoboidControl