From 345c705bd033ce0adf06ed904acc69b59a13e19f Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 2 Jun 2025 12:17:14 +0200 Subject: [PATCH] BB2B rides again (restored lost func. because of merge) --- Unity/DifferentialDrive.cs | 90 +++++++++++++++------------------ Unity/Motor.cs | 4 +- Unity/Participant.cs | 19 ++++--- Unity/Thing.cs | 13 +++-- Unity/TouchSensor.cs | 12 +++-- Unity/Wheel.cs | 16 +++--- src/Things/DifferentialDrive.cs | 32 +----------- src/Things/Motor.cs | 2 +- src/Things/TouchSensor.cs | 15 ++++-- 9 files changed, 96 insertions(+), 107 deletions(-) diff --git a/Unity/DifferentialDrive.cs b/Unity/DifferentialDrive.cs index 4f573aa..2d2c53a 100644 --- a/Unity/DifferentialDrive.cs +++ b/Unity/DifferentialDrive.cs @@ -42,6 +42,8 @@ namespace RoboidControl.Unity { return differentialDrive; } + public Motor leftMotor; + public Motor rightMotor; /// /// The left wheel of the differential drive /// @@ -71,41 +73,8 @@ namespace RoboidControl.Unity { /// Handle the binary event indicating a configuration change /// 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(); - 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(); - leftWheelCollider.radius = coreDrive.wheelRadius; - } - - // Destroy any (generic) thing with the same id - if (rightWheel == null) { - Thing[] things = FindObjectsOfType(); - 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(); - 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(); 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); + } } /// @@ -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; + } } } } diff --git a/Unity/Motor.cs b/Unity/Motor.cs index 3c15cdd..07214fa 100644 --- a/Unity/Motor.cs +++ b/Unity/Motor.cs @@ -34,8 +34,8 @@ namespace RoboidControl.Unity { motor = gameObj.AddComponent(); motor.Init(coreMotor); - Rigidbody rb = gameObj.AddComponent(); - rb.isKinematic = true; + // Rigidbody rb = gameObj.AddComponent(); + // rb.isKinematic = true; } return motor; } diff --git a/Unity/Participant.cs b/Unity/Participant.cs index 62834a8..52539f8 100644 --- a/Unity/Participant.cs +++ b/Unity/Participant.cs @@ -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(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; diff --git a/Unity/Thing.cs b/Unity/Thing.cs index 0e78e9c..a54adb7 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -29,7 +29,7 @@ namespace RoboidControl.Unity { /// The core of the thing /// The created thing 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 { /// 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); + // } } /// @@ -96,10 +99,12 @@ namespace RoboidControl.Unity { /// 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); diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs index 9702ffb..162f50d 100644 --- a/Unity/TouchSensor.cs +++ b/Unity/TouchSensor.cs @@ -45,6 +45,8 @@ namespace RoboidControl.Unity { return touchSensor; } + public bool touchedSomething = false; + /// /// Handle touch trigger collider enter event /// @@ -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)); } /// /// 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; } } diff --git a/Unity/Wheel.cs b/Unity/Wheel.cs index ba0dd24..978a2ff 100644 --- a/Unity/Wheel.cs +++ b/Unity/Wheel.cs @@ -6,13 +6,13 @@ namespace RoboidControl.Unity { /// /// The Unity representation of a Roboid Control wheel /// - public class Wheel : Motor { + public class Wheel : Thing { /// /// Create the Unity representation /// /// The core motor /// The Unity representation of a motorised wheel - 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(); 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(); 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 { diff --git a/src/Things/DifferentialDrive.cs b/src/Things/DifferentialDrive.cs index aa3b4f4..c266d4f 100644 --- a/src/Things/DifferentialDrive.cs +++ b/src/Things/DifferentialDrive.cs @@ -7,40 +7,12 @@ namespace RoboidControl { /// /// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink public class DifferentialDrive : Thing { - /* - /// - /// Create a differential drive without communication abilities - /// Invoke a OnNewThing event when the thing has been created - public DifferentialDrive() : base(Type.DifferentialDrive) { } - - /// - /// Create a differential drive for a participant - /// - /// The owning participant - /// The ID of the thing, leave out or set to zero to generate an ID - /// Invoke a OnNewThing event when the thing has been created - 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)); - } - */ /// - /// Create a new child differential drive + /// Create a new differential drive /// /// The parent thing - /// The ID of the thing, leave out or set to zero to generate an ID - /// Invoke a OnNewThing event when the thing has been created - public DifferentialDrive(Thing parent) : base(parent) { + public DifferentialDrive(Thing parent = default) : base(parent) { this.type = Type.DifferentialDrive; } diff --git a/src/Things/Motor.cs b/src/Things/Motor.cs index 9a9ac7b..a41cc9a 100644 --- a/src/Things/Motor.cs +++ b/src/Things/Motor.cs @@ -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 diff --git a/src/Things/TouchSensor.cs b/src/Things/TouchSensor.cs index 4c1670e..4235c30 100644 --- a/src/Things/TouchSensor.cs +++ b/src/Things/TouchSensor.cs @@ -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 { /// A byte array with the binary data /// The byte array will be empty when the touch status has not changed 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[] 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; /// /// Function used to process binary data received for this touch sensor /// /// The binary data to process public override void ProcessBinary(byte[] bytes) { - this.touchedSomething |= (bytes[0] == 1); + //this.touchedSomething |= (bytes[0] == 1); + this.externalTouch = (bytes[0] == 1); } } } \ No newline at end of file