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