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

BB2B rides again (restored lost func. because of merge)

Browse Source
This commit is contained in:
Pascal Serrarens 2025-06-02 12:17:14 +02:00
parent cf3587b226
commit 345c705bd0
9 changed files with 96 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
Unity/DifferentialDrive.cs
View File

@ -42,6 +42,8 @@ namespace RoboidControl.Unity {
return differentialDrive; return differentialDrive;
} }
public Motor leftMotor;
public Motor rightMotor;
/// <summary> /// <summary>
/// The left wheel of the differential drive /// The left wheel of the differential drive
/// </summary> /// </summary>
@ -71,41 +73,8 @@ namespace RoboidControl.Unity {
/// Handle the binary event indicating a configuration change /// Handle the binary event indicating a configuration change
/// </summary> /// </summary>
protected override void HandleBinary() { protected override void HandleBinary() {
// Ignore it when the wheel radius is not set HandleWheelBinary(coreDrive.leftWheel, ref leftMotor, ref leftWheel);
if (coreDrive.wheelRadius <= 0) HandleWheelBinary(coreDrive.rightWheel, ref rightMotor, ref rightWheel);
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) { if (casterWheel == null) {
GameObject gameObj = new("Caster wheel"); GameObject gameObj = new("Caster wheel");
@ -113,11 +82,30 @@ namespace RoboidControl.Unity {
casterWheel = gameObj.AddComponent<SphereCollider>(); casterWheel = gameObj.AddComponent<SphereCollider>();
casterWheel.material = Wheel.slidingWheel; casterWheel.material = Wheel.slidingWheel;
} }
casterWheel.radius = coreDrive.wheelRadius; if (coreDrive.wheelRadius > 0 && coreDrive.leftWheel != null && coreDrive.rightWheel != null) {
// Put it in the middle of the back casterWheel.radius = coreDrive.wheelRadius;
// This code assumes that the left wheel position has Direction.left and the right wheel Direction.right... // Put it in the middle of the back
float wheelSeparation = coreDrive.leftWheel.position.distance + coreDrive.rightWheel.position.distance; // This code assumes that the left wheel position has Direction.left and the right wheel Direction.right...
casterWheel.center = new Vector3(0, 0, -wheelSeparation); float wheelSeparation = coreDrive.leftWheel.position.distance + coreDrive.rightWheel.position.distance;
casterWheel.center = new Vector3(0, 0, -wheelSeparation);
}
}
private void HandleWheelBinary(RoboidControl.Motor coreMotor, ref Motor motor, ref Wheel wheel) {
if (coreMotor == null)
return;
if (motor == null) {
motor = coreMotor.component as Motor;
if (motor == null)
motor = Motor.Create(coreMotor);
wheel.transform.SetParent(motor.transform);
}
else if (motor.core.id != coreMotor.id) {
motor = coreMotor.component as Motor;
if (motor != null)
wheel.transform.SetParent(motor.transform);
}
} }
/// <summary> /// <summary>
@ -126,18 +114,20 @@ namespace RoboidControl.Unity {
protected override void FixedUpdate() { protected override void FixedUpdate() {
base.FixedUpdate(); base.FixedUpdate();
if (rb != null && leftWheel != null && rightWheel != null) { if (rb != null && leftMotor != null && rightMotor != null) {
float leftWheelVelocity = leftWheel.rotationSpeed * (2 * Mathf.PI) * coreDrive.wheelRadius; float leftWheelVelocity = leftMotor.rotationSpeed * (2 * Mathf.PI) * leftWheel.wheelRadius;
float rightWheelVelocity = rightWheel.rotationSpeed * (2 * Mathf.PI) * coreDrive.wheelRadius; float rightWheelVelocity = rightMotor.rotationSpeed * (2 * Mathf.PI) * rightWheel.wheelRadius;
// This code assumes that the left wheel position has Direction.left and the right wheel Direction.right... if (leftWheel != null && rightWheel != null) {
float wheelSeparation = coreDrive.leftWheel.position.distance + coreDrive.rightWheel.position.distance; float wheelSeparation = Vector3.Distance(leftWheel.transform.position, rightWheel.transform.position);
float forwardSpeed = (leftWheelVelocity + rightWheelVelocity) / 2f; float forwardSpeed = (leftWheelVelocity + rightWheelVelocity) / 2f;
float turningSpeed = (leftWheelVelocity - rightWheelVelocity) / wheelSeparation; float turningSpeed = (leftWheelVelocity - rightWheelVelocity) / 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;
rb.angularVelocity = 0.9f * rb.angularVelocity + 0.1f * turningSpeed * Vector3.up; Debug.Log(rb.velocity);
rb.angularVelocity = 0.9f * rb.angularVelocity + 0.1f * turningSpeed * Vector3.up;
}
} }
} }
} }

4
Unity/Motor.cs
View File

@ -34,8 +34,8 @@ namespace RoboidControl.Unity {
motor = gameObj.AddComponent<Motor>(); motor = gameObj.AddComponent<Motor>();
motor.Init(coreMotor); motor.Init(coreMotor);
Rigidbody rb = gameObj.AddComponent<Rigidbody>(); // Rigidbody rb = gameObj.AddComponent<Rigidbody>();
rb.isKinematic = true; // rb.isKinematic = true;
} }
return motor; return motor;
} }

19
Unity/Participant.cs
View File

@ -1,4 +1,5 @@
using UnityEngine; using UnityEngine;
using System.Linq;
namespace RoboidControl.Unity { namespace RoboidControl.Unity {
@ -36,16 +37,20 @@ namespace RoboidControl.Unity {
DifferentialDrive differentialDrive = DifferentialDrive.Create(coreDrive); DifferentialDrive differentialDrive = DifferentialDrive.Create(coreDrive);
coreDrive.component = differentialDrive; coreDrive.component = differentialDrive;
break; break;
// case RoboidControl.Motor coreMotor: case RoboidControl.Motor coreMotor:
// //Wheel wheel = Wheel.Create(coreMotor); Motor wheel = Motor.Create(coreMotor);
// //coreMotor.component = wheel; coreMotor.component = wheel;
// // We need to know the details (though a binary msg) // We need to know the details (though a binary msg)
// // 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"); Debug.Log("Handle Thing");
if (coreThing.component == null) { if (coreThing.component == null) {
Thing thing = Thing.Create(coreThing); Thing[] things = FindObjectsByType<Thing>(FindObjectsSortMode.None);
Debug.Log(things.Length);
Thing thing = things.FirstOrDefault(t => t.core != null && t.core.id == coreThing.id);
if (thing == null)
thing = Thing.Create(coreThing);
coreThing.component = thing; coreThing.component = thing;
} }
break; break;

13
Unity/Thing.cs
View File

@ -29,7 +29,7 @@ namespace RoboidControl.Unity {
/// <param name="core">The core of the thing</param> /// <param name="core">The core of the thing</param>
/// <returns>The created thing</returns> /// <returns>The created thing</returns>
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) ?
new("Thing") : new("Thing") :
new(core.name); new(core.name);
@ -88,7 +88,10 @@ namespace RoboidControl.Unity {
/// </summary> /// </summary>
protected virtual void FixedUpdate() { protected virtual void FixedUpdate() {
UpdateThing(); UpdateThing();
core.orientation = LinearAlgebra.SwingTwist.FromQuaternion(this.transform.localRotation); // if (core != null) {
// // This is new....
// core.orientation = LinearAlgebra.SwingTwist.FromQuaternion(this.transform.localRotation);
// }
} }
/// <summary> /// <summary>
@ -96,10 +99,12 @@ namespace RoboidControl.Unity {
/// </summary> /// </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...");
Destroy(this); // Destroy(this);
return; return;
} }
if (core.updateQueue == null)
return;
while (core.updateQueue.TryDequeue(out RoboidControl.Thing.CoreEvent e)) while (core.updateQueue.TryDequeue(out RoboidControl.Thing.CoreEvent e))
HandleCoreEvent(e); HandleCoreEvent(e);

12
Unity/TouchSensor.cs
View File

@ -45,6 +45,8 @@ namespace RoboidControl.Unity {
return touchSensor; return touchSensor;
} }
public bool touchedSomething = false;
/// <summary> /// <summary>
/// Handle touch trigger collider enter event /// Handle touch trigger collider enter event
/// </summary> /// </summary>
@ -52,13 +54,15 @@ namespace RoboidControl.Unity {
private void OnTriggerEnter(Collider other) { private void OnTriggerEnter(Collider other) {
// Don't detect trigger colliders // Don't detect trigger colliders
if (other.isTrigger) if (other.isTrigger)
return; return;
// Don't touch yourself // Don't touch yourself
if (this.transform.root == other.transform.root) if (this.transform.root == other.transform.root)
return; return;
Debug.Log("TOUCH!");
this.touchedSomething = true;
this.coreSensor.touchedSomething = true; this.coreSensor.touchedSomething = true;
this.core.updateQueue.Enqueue(new RoboidControl.Thing.CoreEvent(BinaryMsg.Id)); //this.core.updateQueue.Enqueue(new RoboidControl.Thing.CoreEvent(BinaryMsg.Id));
} }
/// <summary> /// <summary>
/// Handl touch trigger collider exit event /// Handl touch trigger collider exit event
@ -68,6 +72,8 @@ namespace RoboidControl.Unity {
if (other.isTrigger) if (other.isTrigger)
return; return;
Debug.Log("TOUCH END!");
this.touchedSomething = false;
this.coreSensor.touchedSomething = false; this.coreSensor.touchedSomething = false;
} }
} }

16
Unity/Wheel.cs
View File

@ -6,13 +6,13 @@ namespace RoboidControl.Unity {
/// <summary> /// <summary>
/// The Unity representation of a Roboid Control wheel /// The Unity representation of a Roboid Control wheel
/// </summary> /// </summary>
public class Wheel : Motor { public class Wheel : Thing {
/// <summary> /// <summary>
/// Create the Unity representation /// Create the Unity representation
/// </summary> /// </summary>
/// <param name="core">The core motor</param> /// <param name="core">The core motor</param>
/// <returns>The Unity representation of a motorised wheel</returns> /// <returns>The Unity representation of a motorised wheel</returns>
public static Wheel Create(RoboidControl.Motor core, float wheelRadius) { public static new Wheel Create(RoboidControl.Thing core) {
GameObject prefab = (GameObject)Resources.Load("Wheel"); GameObject prefab = (GameObject)Resources.Load("Wheel");
if (prefab != null) { if (prefab != null) {
// Use resource prefab when available // Use resource prefab when available
@ -38,10 +38,11 @@ namespace RoboidControl.Unity {
GameObject gameObj = Instantiate(prefab); GameObject gameObj = Instantiate(prefab);
Wheel component = gameObj.GetComponent<Wheel>(); Wheel component = gameObj.GetComponent<Wheel>();
if (component != null) { if (component != null) {
component.core = new RoboidControl.Thing { component.core = new RoboidControl.Thing() {
type = RoboidControl.Thing.Type.UncontrolledMotor type = RoboidControl.Thing.Type.UncontrolledMotor
}; };
} }
//component.core = new RoboidControl.Thing(RoboidControl.Thing.Type.UncontrolledMotor, false);
return component; return component;
} }
else { else {
@ -52,13 +53,12 @@ namespace RoboidControl.Unity {
SiteServer participant = FindAnyObjectByType<SiteServer>(); SiteServer participant = FindAnyObjectByType<SiteServer>();
RoboidControl.Thing core = participant.coreParticipant.Get(thingId); RoboidControl.Thing core = participant.coreParticipant.Get(thingId);
if (core == null) { if (core == null) {
//core = new(participant.coreParticipant, RoboidControl.Thing.Type.UncontrolledMotor, thingId, false); core = new(participant.coreParticipant.root) {
//core = RoboidControl.Thing.CreateRemote(participant.coreParticipant, thingId);
core = new RoboidControl.Thing(participant.coreParticipant.root) {
id = thingId, id = thingId,
type = RoboidControl.Thing.Type.UncontrolledMotor type = RoboidControl.Thing.Type.UncontrolledMotor,
}; };
} }
//core = new(participant.coreParticipant, RoboidControl.Thing.Type.UncontrolledMotor, thingId, false);
else { else {
; ;
} }
@ -71,6 +71,8 @@ namespace RoboidControl.Unity {
} }
} }
public float wheelRadius = 0;
private static PhysicMaterial _slidingWheel; private static PhysicMaterial _slidingWheel;
public static PhysicMaterial slidingWheel { public static PhysicMaterial slidingWheel {
get { get {

32
src/Things/DifferentialDrive.cs
View File

@ -7,40 +7,12 @@ namespace RoboidControl {
/// ///
/// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink /// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink
public class DifferentialDrive : Thing { public class DifferentialDrive : Thing {
/*
/// <summary>
/// Create a differential drive without communication abilities
/// </summary
/// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
public DifferentialDrive() : base(Type.DifferentialDrive) { }
/// <summary>
/// Create a differential drive for a participant
/// </summary>
/// <param name="owner">The owning participant</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>
public DifferentialDrive(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.DifferentialDrive, thingId, invokeEvent) {
// Motor leftWheel = new(this) {
// name = "Left Wheel"
// };
// Motor rightWheel = new(this) {
// name = "Right Wheel"
// };
// SetMotors(leftWheel, rightWheel);
// sendBinary = true;
// owner.Send(new BinaryMsg(owner.networkId, this));
// this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id));
}
*/
/// <summary> /// <summary>
/// Create a new child differential drive /// Create a new differential drive
/// </summary> /// </summary>
/// <param name="parent">The parent thing</param> /// <param name="parent">The parent thing</param>
/// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param> public DifferentialDrive(Thing parent = default) : base(parent) {
/// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
public DifferentialDrive(Thing parent) : base(parent) {
this.type = Type.DifferentialDrive; this.type = Type.DifferentialDrive;
} }

2
src/Things/Motor.cs
View File

@ -3,9 +3,9 @@ using LinearAlgebra;
namespace RoboidControl { namespace RoboidControl {
public class Motor : Thing { public class Motor : Thing {
//public Motor(bool invokeEvent = true) : base(Type.UncontrolledMotor, invokeEvent) { }
public Motor(Thing parent) : base(parent) { public Motor(Thing parent) : base(parent) {
this.type = Type.UncontrolledMotor; this.type = Type.UncontrolledMotor;
this.name = "Motor";
} }
/// @brief Motor turning direction /// @brief Motor turning direction

15
src/Things/TouchSensor.cs
View File

@ -38,6 +38,7 @@ namespace RoboidControl {
set { set {
if (_touchedSomething != value) { if (_touchedSomething != value) {
_touchedSomething = value; _touchedSomething = value;
owner.Send(new BinaryMsg(this));
} }
touchUpdated = true; touchUpdated = true;
} }
@ -58,21 +59,29 @@ namespace RoboidControl {
/// <returns>A byte array with the binary data</returns> /// <returns>A byte array with the binary data</returns>
/// <remark>The byte array will be empty when the touch status has not changed</remark> /// <remark>The byte array will be empty when the touch status has not changed</remark>
public override byte[] GenerateBinary() { public override byte[] GenerateBinary() {
if (!touchUpdated) #if UNITY_5_3_OR_NEWER
// We use the unity component because we only want to send the state of what is generated in the simulation
Unity.TouchSensor touchComponent = this.component as Unity.TouchSensor;
if (touchComponent == null || !touchUpdated)
return Array.Empty<byte>(); return Array.Empty<byte>();
byte[] bytes = new byte[1]; byte[] bytes = new byte[1];
bytes[0] = (byte)(touchedSomething ? 1 : 0); bytes[0] = (byte)(touchComponent.touchedSomething ? 1 : 0);
touchUpdated = false; touchUpdated = false;
return bytes; return bytes;
#else
return 0;
#endif
} }
private bool externalTouch = false;
/// <summary> /// <summary>
/// Function used to process binary data received for this touch sensor /// Function used to process binary data received for this touch sensor
/// </summary> /// </summary>
/// <param name="bytes">The binary data to process</param> /// <param name="bytes">The binary data to process</param>
public override void ProcessBinary(byte[] bytes) { public override void ProcessBinary(byte[] bytes) {
this.touchedSomething |= (bytes[0] == 1); //this.touchedSomething |= (bytes[0] == 1);
this.externalTouch = (bytes[0] == 1);
} }
} }
} }