diff --git a/Examples/BB2B/BB2B.cs b/Examples/BB2B/BB2B.cs
index 3d5b77a..6462486 100644
--- a/Examples/BB2B/BB2B.cs
+++ b/Examples/BB2B/BB2B.cs
@@ -1,39 +1,40 @@
namespace RoboidControl {
-public class BB2B : Thing {
- readonly DifferentialDrive drive;
- readonly TouchSensor touchLeft;
- readonly TouchSensor touchRight;
+ // The robot is based on a differential drive
+ public class BB2B : DifferentialDrive {
+ readonly DifferentialDrive drive;
+ readonly TouchSensor touchLeft;
+ readonly TouchSensor touchRight;
- public BB2B() : base(128) { // thingType = 128
+ public BB2B(Participant owner) : base(owner) {
+ this.name = "BB2B";
+ this.wheelRadius = 0.032f;
+ this.wheelSeparation = 0.128f;
- // The robot's propulsion is a differential drive
- drive = new();
+ // Is has a touch sensor at the front left of the roboid
+ touchLeft = new(this);
+ // and other one on the right
+ touchRight = new(this);
+ }
- // Is has a touch sensor at the front left of the roboid
- touchLeft = new(drive);
- // and other one on the right
- touchRight = new(drive);
+ 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 leftWheelSpeed = touchRight.touchedSomething ? -600.0f : 600.0f;
+
+ // The right wheel does the same, but instead is controlled by
+ // touches on the left side
+ float rightWheelSpeed = touchLeft.touchedSomething ? -600.0f : 600.0f;
+
+ // When both sides are touching something, both wheels will turn backward
+ // and the roboid will move backwards
+
+ this.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
+
+ base.Update(currentTimeMs, recurse);
+ }
}
- 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 leftWheelSpeed = touchRight.touchedSomething ? -600.0f : 600.0f;
-
- // The right wheel does the same, but instead is controlled by
- // touches on the left side
- float rightWheelSpeed = touchLeft.touchedSomething ? -600.0f : 600.0f;
-
- // When both sides are touching something, both wheels will turn backward
- // and the roboid will move backwards
-
- drive.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
-
- base.Update(currentTimeMs, recurse);
- }
}
-
-}
\ No newline at end of file
diff --git a/Examples/BB2B/Program.cs b/Examples/BB2B/Program.cs
index 7550097..10b0c67 100644
--- a/Examples/BB2B/Program.cs
+++ b/Examples/BB2B/Program.cs
@@ -3,7 +3,7 @@ using RoboidControl;
class Program {
static void Main() {
- BB2B bb2b = new();
+ BB2B bb2b = new(ParticipantUDP.Isolated());
while (true) {
bb2b.Update();
diff --git a/Unity/DifferentialDrive.cs b/Unity/DifferentialDrive.cs
new file mode 100644
index 0000000..c98c66c
--- /dev/null
+++ b/Unity/DifferentialDrive.cs
@@ -0,0 +1,108 @@
+#if UNITY_5_3_OR_NEWER
+using System.Linq;
+using UnityEngine;
+
+namespace RoboidControl.Unity {
+ public class DifferentialDrive : Thing {
+
+ public WheelCollider leftWheel;
+ public WheelCollider rightWheel;
+
+ ///
+ /// Create the Unity representation
+ ///
+ /// The core touch sensor
+ /// The Unity representation of the touch sensor
+ public static DifferentialDrive Create(RoboidControl.DifferentialDrive core) {
+ GameObject gameObj = new(core.name);
+ DifferentialDrive component = gameObj.AddComponent();
+ component.Init(core);
+
+ Rigidbody rb = gameObj.AddComponent();
+ rb.isKinematic = false;
+ rb.mass = 0.5f;
+
+ return component;
+ }
+
+ protected override void HandleBinary() {
+ RoboidControl.DifferentialDrive drive = core as RoboidControl.DifferentialDrive;
+ if (leftWheel == null) {
+ GameObject leftWheelObj = new GameObject("Left wheel");
+ leftWheelObj.transform.SetParent(this.transform);
+ leftWheel = leftWheelObj.AddComponent();
+ leftWheel.mass = 0.1f;
+ leftWheel.suspensionDistance = 0.01f;
+ leftWheel.suspensionSpring = new JointSpring {
+ spring = 1000f, // Very high spring value to make it rigid
+ damper = 100f, // Low damping (could be adjusted for slight 'bounciness')
+ targetPosition = 0.5f // Neutral position (middle of the suspension travel)
+ };
+ leftWheel.radius = drive.wheelRadius;
+ leftWheel.center = new Vector3(-drive.wheelSeparation / 2, 0, 0);
+ }
+ if (rightWheel == null) {
+ GameObject rightWheelObj = new GameObject("Left wheel");
+ rightWheelObj.transform.SetParent(this.transform);
+ rightWheel = rightWheelObj.AddComponent();
+ rightWheel.mass = 0.1f;
+ rightWheel.suspensionDistance = 0.01f;
+ rightWheel.suspensionSpring = new JointSpring {
+ spring = 1000f, // Very high spring value to make it rigid
+ damper = 100f, // Low damping (could be adjusted for slight 'bounciness')
+ targetPosition = 0.5f // Neutral position (middle of the suspension travel)
+ };
+ rightWheel.radius = drive.wheelRadius;
+ rightWheel.center = new Vector3(drive.wheelSeparation / 2, 0, 0);
+
+ }
+ // Thing[] children = this.GetComponentsInChildren();
+ // if (leftWheel == null) {
+ // leftWheel = children.FirstOrDefault(child => child.core.id == drive.leftWheel.id);
+ // if (leftWheel == null) {
+ // RoboidControl.Thing coreThing = new(drive.owner, 0, drive.leftWheel.id, false) {
+ // name = "Left Wheel"
+ // };
+ // leftWheel = Thing.Create(coreThing);
+ // leftWheel.transform.SetParent(this.transform);
+ // }
+ // WheelCollider wheel = this.GetComponent();
+ // if (wheel == null)
+ // wheel = this.gameObject.AddComponent();
+ // wheel.mass = 0.1f;
+ // wheel.suspensionDistance = 0.01f;
+ // wheel.suspensionSpring = new JointSpring {
+ // spring = 1000f, // Very high spring value to make it rigid
+ // damper = 100f, // Low damping (could be adjusted for slight 'bounciness')
+ // targetPosition = 0.5f // Neutral position (middle of the suspension travel)
+ // };
+ // wheel.radius = drive.wheelRadius;
+ // wheel.center = new Vector3(-drive.wheelSeparation / 2, 0, 0);
+ // }
+ // if (rightWheel == null) {
+ // this.rightWheel = children.FirstOrDefault(child => child.core.id == drive.rightWheel.id);
+ // if (this.rightWheel == null) {
+ // RoboidControl.Thing coreThing = new(drive.owner, 0, drive.rightWheel.id, false) {
+ // name = "Right Wheel"
+ // };
+ // this.rightWheel = Thing.Create(coreThing);
+ // this.rightWheel.transform.SetParent(this.transform);
+ // }
+ // WheelCollider wheel = this.GetComponent();
+ // if (wheel == null)
+ // wheel = this.gameObject.AddComponent();
+ // wheel.mass = 0.1f;
+ // wheel.suspensionDistance = 0.01f;
+ // wheel.suspensionSpring = new JointSpring {
+ // spring = 1000f, // Very high spring value to make it rigid
+ // damper = 100f, // Low damping (could be adjusted for slight 'bounciness')
+ // targetPosition = 0.5f // Neutral position (middle of the suspension travel)
+ // };
+ // wheel.radius = drive.wheelRadius;
+ // wheel.center = new Vector3(drive.wheelSeparation / 2, 0, 0);
+ // }
+ }
+ }
+
+}
+#endif
\ No newline at end of file
diff --git a/Unity/Participant.cs b/Unity/Participant.cs
index 1921ef2..c51aadb 100644
--- a/Unity/Participant.cs
+++ b/Unity/Participant.cs
@@ -24,15 +24,21 @@ namespace RoboidControl.Unity {
}
}
- private void HandleThingEvent(RoboidControl.Participant.UpdateEvent e) {
+ protected virtual void HandleThingEvent(RoboidControl.Participant.UpdateEvent e) {
switch (e.thing) {
case RoboidControl.TouchSensor coreTouchSensor:
TouchSensor touchSensor = TouchSensor.Create(coreTouchSensor);
coreTouchSensor.component = touchSensor;
break;
+ case RoboidControl.DifferentialDrive coreDrive:
+ DifferentialDrive differentialDrive = DifferentialDrive.Create(coreDrive);
+ coreDrive.component = differentialDrive;
+ break;
case RoboidControl.Thing coreThing:
- Thing thing = Thing.Create(coreThing);
- coreThing.component = thing;
+ if (coreThing.component != null) {
+ Thing thing = Thing.Create(coreThing);
+ coreThing.component = thing;
+ }
break;
}
}
diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs
index b260f43..76dd34c 100644
--- a/Unity/SiteServer.cs
+++ b/Unity/SiteServer.cs
@@ -42,7 +42,8 @@ namespace RoboidControl.Unity {
participant.coreParticipant = e.participant;
break;
case ThingMsg.id:
- e.thing.CreateComponent();
+ HandleThingEvent(e);
+ //e.thing.CreateComponent();
break;
}
}
diff --git a/Unity/Thing.cs b/Unity/Thing.cs
index 700ca87..7a38bd4 100644
--- a/Unity/Thing.cs
+++ b/Unity/Thing.cs
@@ -48,6 +48,7 @@ namespace RoboidControl.Unity {
protected void Init(RoboidControl.Thing core) {
this.core = core;
this.participant = FindAnyObjectByType();
+ core.owner = this.participant.coreParticipant;
if (core.parent != null && core.parent.component != null)
this.transform.SetParent(core.parent.component.transform, false);
@@ -105,6 +106,9 @@ namespace RoboidControl.Unity {
if (core.angularVelocity.distance == 0)
this.transform.localRotation = core.orientation.ToQuaternion();
break;
+ case BinaryMsg.Id:
+ this.HandleBinary();
+ break;
}
}
@@ -143,6 +147,7 @@ namespace RoboidControl.Unity {
}
}
+ protected virtual void HandleBinary() {}
}
diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs
index 0c4e593..8a1fe79 100644
--- a/Unity/TouchSensor.cs
+++ b/Unity/TouchSensor.cs
@@ -36,9 +36,7 @@ namespace RoboidControl.Unity {
/// The core touch sensor
/// The Unity representation of the touch sensor
public static TouchSensor Create(RoboidControl.TouchSensor core) {
- GameObject gameObj = core.name != null ?
- new(core.name) :
- new("Touch Sensor");
+ GameObject gameObj = new(core.name);
TouchSensor component = gameObj.AddComponent();
component.Init(core);
diff --git a/src/Participants/SiteServer.cs b/src/Participants/SiteServer.cs
index 91b0b8b..8c38572 100644
--- a/src/Participants/SiteServer.cs
+++ b/src/Participants/SiteServer.cs
@@ -117,16 +117,7 @@ namespace RoboidControl {
Console.WriteLine($"{this.name}: Process thing [{msg.networkId}/{msg.thingId}] {msg.thingType} {msg.parentId} ");
Thing thing = sender.Get(msg.thingId);
- if (thing == null) {
- switch (msg.thingType) {
- case (byte)Thing.Type.TouchSensor:
- new TouchSensor(sender, msg.thingId);
- break;
- }
- }
-
- if (thing == null)
- thing = new Thing(sender, msg.thingType, msg.thingId);
+ thing ??= ProcessNewThing(sender, msg);
if (msg.parentId != 0) {
thing.parent = sender.Get(msg.parentId);
@@ -139,6 +130,14 @@ namespace RoboidControl {
}
}
+ protected virtual Thing ProcessNewThing(Participant sender, ThingMsg msg) {
+ return msg.thingType switch {
+ Thing.Type.TouchSensor => new TouchSensor(sender, msg.thingId),
+ Thing.Type.DifferentialDrive => new DifferentialDrive(sender, msg.thingId),
+ _ => new Thing(sender, msg.thingType, msg.thingId),
+ };
+
+ }
#endregion Receive
diff --git a/src/Things/DifferentialDrive.cs b/src/Things/DifferentialDrive.cs
index f0cc4ce..c9db363 100644
--- a/src/Things/DifferentialDrive.cs
+++ b/src/Things/DifferentialDrive.cs
@@ -17,7 +17,19 @@ namespace RoboidControl {
/// 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 participant, byte thingId = 0, bool invokeEvent = true) : base(participant, Type.DifferentialDrive, thingId, invokeEvent) { }
+ public DifferentialDrive(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.DifferentialDrive, thingId, invokeEvent) {
+ Thing leftWheel = new(this) {
+ name = "Left Wheel"
+ };
+ Thing 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
///
@@ -92,17 +104,43 @@ namespace RoboidControl {
}
/// @brief The radius of a wheel in meters
- protected float wheelRadius = 1.0f;
+ public float wheelRadius = 1.0f;
/// @brief The distance between the wheels in meters
- protected float wheelSeparation = 1.0f;
+ public float wheelSeparation = 1.0f;
/// @brief Convert revolutions per second to meters per second
protected float rpsToMs = 1.0f;
/// @brief The left wheel
- protected Thing leftWheel = null;
+ public Thing leftWheel = null;
/// @brief The right wheel
- protected Thing rightWheel = null;
+ public Thing rightWheel = null;
+
+ bool sendBinary = false;
+ public override byte[] GenerateBinary() {
+ if (!sendBinary)
+ return System.Array.Empty();
+
+ byte[] data = new byte[6];
+ byte ix = 0;
+ data[ix++] = leftWheel.id;
+ data[ix++] = rightWheel.id;
+ LowLevelMessages.SendFloat16(data, ref ix, wheelRadius);
+ LowLevelMessages.SendFloat16(data, ref ix, wheelSeparation);
+ sendBinary = false;
+ return data;
+ }
+
+ public override void ProcessBinary(byte[] data) {
+ byte ix = 0;
+ byte leftWheelId = data[ix++];
+ this.leftWheel = this.owner.Get(leftWheelId);
+ byte rightWheelId = data[ix++];
+ this.rightWheel = this.owner.Get(rightWheelId);
+ this.wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix);
+ this.wheelSeparation = LowLevelMessages.ReceiveFloat16(data, ref ix);
+ this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id));
+ }
};
} // namespace RoboidControl
\ No newline at end of file
diff --git a/src/Things/TouchSensor.cs b/src/Things/TouchSensor.cs
index 60e050f..2198e6e 100644
--- a/src/Things/TouchSensor.cs
+++ b/src/Things/TouchSensor.cs
@@ -24,7 +24,9 @@ namespace RoboidControl {
/// 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 TouchSensor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.TouchSensor, thingId, invokeEvent) { }
+ public TouchSensor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.TouchSensor, thingId, invokeEvent) {
+ this.name = "TouchSensor";
+ }
///
/// Value which is true when the sensor is touching something, false otherwise