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;
}
public Motor leftMotor;
public Motor rightMotor;
/// <summary>
/// The left wheel of the differential drive
/// </summary>
@ -71,41 +73,8 @@ namespace RoboidControl.Unity {
/// 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;
}
HandleWheelBinary(coreDrive.leftWheel, ref leftMotor, ref leftWheel);
HandleWheelBinary(coreDrive.rightWheel, ref rightMotor, ref rightWheel);
if (casterWheel == null) {
GameObject gameObj = new("Caster wheel");
@ -113,11 +82,30 @@ namespace RoboidControl.Unity {
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);
if (coreDrive.wheelRadius > 0 && coreDrive.leftWheel != null && coreDrive.rightWheel != null) {
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);
}
}
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>
@ -126,18 +114,20 @@ namespace RoboidControl.Unity {
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;
if (rb != null && leftMotor != null && rightMotor != null) {
float leftWheelVelocity = leftMotor.rotationSpeed * (2 * Mathf.PI) * leftWheel.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...
float wheelSeparation = coreDrive.leftWheel.position.distance + coreDrive.rightWheel.position.distance;
float forwardSpeed = (leftWheelVelocity + rightWheelVelocity) / 2f;
float turningSpeed = (leftWheelVelocity - rightWheelVelocity) / wheelSeparation;
if (leftWheel != null && rightWheel != null) {
float wheelSeparation = Vector3.Distance(leftWheel.transform.position, rightWheel.transform.position);
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;
// Use smoothing to emulate motor inertia
rb.velocity = 0.9f * rb.velocity + 0.1f * forwardSpeed * transform.forward;
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.Init(coreMotor);
Rigidbody rb = gameObj.AddComponent<Rigidbody>();
rb.isKinematic = true;
// Rigidbody rb = gameObj.AddComponent<Rigidbody>();
// rb.isKinematic = true;
}
return motor;
}

19
Unity/Participant.cs
View File

@ -1,4 +1,5 @@
using UnityEngine;
using System.Linq;
namespace RoboidControl.Unity {
@ -36,16 +37,20 @@ namespace RoboidControl.Unity {
DifferentialDrive differentialDrive = DifferentialDrive.Create(coreDrive);
coreDrive.component = differentialDrive;
break;
// case RoboidControl.Motor coreMotor:
// //Wheel wheel = Wheel.Create(coreMotor);
// //coreMotor.component = wheel;
// // We need to know the details (though a binary msg)
// // before we can create the wheel reliably
// break;
case RoboidControl.Motor coreMotor:
Motor wheel = Motor.Create(coreMotor);
coreMotor.component = wheel;
// We need to know the details (though a binary msg)
// before we can create the wheel reliably
break;
case RoboidControl.Thing coreThing:
Debug.Log("Handle Thing");
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;
}
break;

13
Unity/Thing.cs
View File

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

12
Unity/TouchSensor.cs
View File

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

16
Unity/Wheel.cs
View File

@ -6,13 +6,13 @@ namespace RoboidControl.Unity {
/// <summary>
/// The Unity representation of a Roboid Control wheel
/// </summary>
public class Wheel : Motor {
public class Wheel : Thing {
/// <summary>
/// Create the Unity representation
/// </summary>
/// <param name="core">The core motor</param>
/// <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");
if (prefab != null) {
// Use resource prefab when available
@ -38,10 +38,11 @@ namespace RoboidControl.Unity {
GameObject gameObj = Instantiate(prefab);
Wheel component = gameObj.GetComponent<Wheel>();
if (component != null) {
component.core = new RoboidControl.Thing {
component.core = new RoboidControl.Thing() {
type = RoboidControl.Thing.Type.UncontrolledMotor
};
}
//component.core = new RoboidControl.Thing(RoboidControl.Thing.Type.UncontrolledMotor, false);
return component;
}
else {
@ -52,13 +53,12 @@ namespace RoboidControl.Unity {
SiteServer participant = FindAnyObjectByType<SiteServer>();
RoboidControl.Thing core = participant.coreParticipant.Get(thingId);
if (core == null) {
//core = new(participant.coreParticipant, RoboidControl.Thing.Type.UncontrolledMotor, thingId, false);
//core = RoboidControl.Thing.CreateRemote(participant.coreParticipant, thingId);
core = new RoboidControl.Thing(participant.coreParticipant.root) {
core = new(participant.coreParticipant.root) {
id = thingId,
type = RoboidControl.Thing.Type.UncontrolledMotor
type = RoboidControl.Thing.Type.UncontrolledMotor,
};
}
//core = new(participant.coreParticipant, RoboidControl.Thing.Type.UncontrolledMotor, thingId, false);
else {
;
}
@ -71,6 +71,8 @@ namespace RoboidControl.Unity {
}
}
public float wheelRadius = 0;
private static PhysicMaterial _slidingWheel;
public static PhysicMaterial slidingWheel {
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
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>
/// Create a new child differential drive
/// Create a new differential drive
/// </summary>
/// <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>
/// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
public DifferentialDrive(Thing parent) : base(parent) {
public DifferentialDrive(Thing parent = default) : base(parent) {
this.type = Type.DifferentialDrive;
}

2
src/Things/Motor.cs
View File

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

15
src/Things/TouchSensor.cs
View File

@ -38,6 +38,7 @@ namespace RoboidControl {
set {
if (_touchedSomething != value) {
_touchedSomething = value;
owner.Send(new BinaryMsg(this));
}
touchUpdated = true;
}
@ -58,21 +59,29 @@ namespace RoboidControl {
/// <returns>A byte array with the binary data</returns>
/// <remark>The byte array will be empty when the touch status has not changed</remark>
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>();
byte[] bytes = new byte[1];
bytes[0] = (byte)(touchedSomething ? 1 : 0);
bytes[0] = (byte)(touchComponent.touchedSomething ? 1 : 0);
touchUpdated = false;
return bytes;
#else
return 0;
#endif
}
private bool externalTouch = false;
/// <summary>
/// Function used to process binary data received for this touch sensor
/// </summary>
/// <param name="bytes">The binary data to process</param>
public override void ProcessBinary(byte[] bytes) {
this.touchedSomething |= (bytes[0] == 1);
//this.touchedSomething |= (bytes[0] == 1);
this.externalTouch = (bytes[0] == 1);
}
}
}