diff --git a/src/Participants/SiteServer.cs b/src/Participants/SiteServer.cs
index 7e830df..fc3d2d9 100644
--- a/src/Participants/SiteServer.cs
+++ b/src/Participants/SiteServer.cs
@@ -109,7 +109,7 @@ namespace RoboidControl {
if (thing == null) {
switch (msg.thingType) {
case (byte)Thing.Type.TouchSensor:
- new TouchSensor(sender, msg.networkId, msg.thingId);
+ new TouchSensor(sender, msg.thingId);
break;
}
}
diff --git a/src/Thing.cs b/src/Thing.cs
index 56f8ad7..6ea166e 100644
--- a/src/Thing.cs
+++ b/src/Thing.cs
@@ -27,9 +27,10 @@ namespace RoboidControl {
public const byte Servo = 0x08;
// Other
public const byte Roboid = 0x09;
- public const byte HUmanoid = 0x010;
- public const byte ExternalSensor = 0x11;
- public const byte Animator = 0x40;
+ public const byte HUmanoid = 0x0A;
+ public const byte ExternalSensor = 0x0B;
+ public const byte Animator = 0x0C;
+ public const byte DifferentialDrive = 0x0D;
}
#region Init
@@ -67,7 +68,7 @@ namespace RoboidControl {
/// 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
/// The owner will be the same as the owner of the parent thing
- public Thing(Thing parent, byte thingType = Type.Undetermined, bool invokeEvent = true) : this(parent.owner, thingType, 0, invokeEvent) {
+ public Thing(Thing parent, byte thingType = Type.Undetermined, byte thingId = 0, bool invokeEvent = true) : this(parent.owner, thingType, thingId, invokeEvent) {
this.parent = parent;
}
@@ -424,7 +425,7 @@ namespace RoboidControl {
///
/// Function used to process binary data received for this thing
///
- /// The binary data
+ /// The binary data to process
public virtual void ProcessBinary(byte[] bytes) {
}
diff --git a/src/Things/DifferentialDrive.cs b/src/Things/DifferentialDrive.cs
index f47ea3a..a2a5621 100644
--- a/src/Things/DifferentialDrive.cs
+++ b/src/Things/DifferentialDrive.cs
@@ -1,14 +1,30 @@
+using LinearAlgebra;
+
namespace RoboidControl {
/// @brief A thing which can move itself using a differential drive system
///
/// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink
public class DifferentialDrive : Thing {
- /// @brief Create a differential drive without networking support
- public DifferentialDrive() { }
- /// @brief Create a differential drive with networking support
- /// @param participant The local participant
- public DifferentialDrive(ParticipantUDP participant) : base(participant, Type.Undetermined) { }
+ ///
+ /// Create a differential drive without communication abilities
+ /// Invoke a OnNewThing event when the thing has been created
+ public DifferentialDrive(bool invokeEvent = true) : base(Type.DifferentialDrive, invokeEvent) { }
+ ///
+ /// 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(ParticipantUDP participant, byte thingId = 0, bool invokeEvent = true) : base(participant, Type.DifferentialDrive, thingId, invokeEvent) { }
+ ///
+ /// Create a new child 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, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.DifferentialDrive, thingId, invokeEvent) { }
/// @brief Configures the dimensions of the drive
/// @param wheelDiameter The diameter of the wheels in meters
@@ -17,21 +33,63 @@ namespace RoboidControl {
/// These values are used to compute the desired wheel speed from the set
/// linear and angular velocity.
/// @sa SetLinearVelocity SetAngularVelocity
- public void SetDriveDimensions(float wheelDiameter, float wheelSeparation) { }
+ public void SetDriveDimensions(float wheelDiameter, float wheelSeparation) {
+ this.wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
+ this.wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
+ this.rpsToMs = wheelDiameter * Angle.pi;
+
+ float distance = this.wheelSeparation / 2;
+ if (this.leftWheel != null)
+ this.leftWheel.position = new Spherical(distance, Direction.left);
+ if (this.rightWheel != null)
+ this.rightWheel.position = new Spherical(distance, Direction.right);
+ }
+
/// @brief Congures the motors for the wheels
/// @param leftWheel The motor for the left wheel
/// @param rightWheel The motor for the right wheel
- public void SetMotors(Thing leftWheel, Thing rightWheel) { }
+ public void SetMotors(Thing leftWheel, Thing rightWheel) {
+ float distance = this.wheelSeparation / 2;
+
+ this.leftWheel = leftWheel;
+ if (this.leftWheel != null)
+ this.leftWheel.position = new Spherical(distance, Direction.left);
+
+ this.rightWheel = rightWheel;
+ if (this.rightWheel != null)
+ this.rightWheel.position = new Spherical(distance, Direction.right);
+ }
/// @brief Directly specify the speeds of the motors
/// @param speedLeft The speed of the left wheel in degrees per second.
/// Positive moves the robot in the forward direction.
/// @param speedRight The speed of the right wheel in degrees per second.
/// Positive moves the robot in the forward direction.
- public void SetWheelVelocity(float speedLeft, float speedRight) { }
+ public void SetWheelVelocity(float speedLeft, float speedRight) {
+ if (this.leftWheel != null)
+ this.leftWheel.angularVelocity = new Spherical(speedLeft, Direction.left);
+ if (this.rightWheel != null)
+ this.rightWheel.angularVelocity = new Spherical(speedRight, Direction.right);
+ }
/// @copydoc RoboidControl::Thing::Update(unsigned long)
- public override void Update(ulong currentMs, bool recursive = true) { }
+ public override void Update(ulong currentMs, bool recursive = true) {
+ if (this.linearVelocityUpdated) {
+ // this assumes forward velocity only....
+ float linearVelocity = this.linearVelocity.distance;
+
+ Spherical angularVelocity = this.angularVelocity;
+ float angularSpeed = angularVelocity.distance * Angle.Deg2Rad;
+ // Determine the rotation direction
+ if (angularVelocity.direction.horizontal < 0)
+ angularSpeed = -angularSpeed;
+
+ // wheel separation can be replaced by this.leftwheel.position.distance
+ float speedLeft = (linearVelocity + angularSpeed * this.wheelSeparation / 2) / this.wheelRadius * Angle.Rad2Deg;
+ float speedRight = (linearVelocity - angularSpeed * this.wheelSeparation / 2) / this.wheelRadius * Angle.Rad2Deg;
+ this.SetWheelVelocity(speedLeft, speedRight);
+ }
+ }
/// @brief The radius of a wheel in meters
protected float wheelRadius = 1.0f;
diff --git a/src/Things/DigitalSensor.cs b/src/Things/DigitalSensor.cs
new file mode 100644
index 0000000..7942219
--- /dev/null
+++ b/src/Things/DigitalSensor.cs
@@ -0,0 +1,74 @@
+using System;
+
+namespace RoboidControl {
+
+ ///
+ /// A sensor which can detect touches
+ ///
+ public class DigitalSensor : Thing {
+ ///
+ /// Create a digital sensor without communication abilities
+ ///
+ /// Invoke a OnNewThing event when the thing has been created
+ public DigitalSensor(bool invokeEvent = true) : base(Type.Switch, invokeEvent) { }
+ ///
+ /// Create a digital sensor 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 DigitalSensor(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.Switch, thingId, invokeEvent) { }
+ ///
+ /// Create a new child digital sensor
+ ///
+ /// 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 DigitalSensor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.Switch, thingId, invokeEvent) { }
+
+ ///
+ /// Value which is true when the sensor is touching something, false otherwise
+ ///
+ private bool _state = false;
+ public bool state {
+ get { return _state; }
+ set {
+ if (_state != value) {
+ _state = value;
+ }
+ stateUpdated = true;
+ }
+ }
+ private bool stateUpdated = false;
+
+#if UNITY_5_3_OR_NEWER
+ /// @copydoc Passer::RoboidControl::Thing::CreateComponent
+ public override void CreateComponent() {
+ this.component = Unity.DigitalSensor.Create(this);
+ this.component.core = this;
+ }
+#endif
+ ///
+ /// Function used to generate binary data for this digital sensor
+ ///
+ /// A byte array with the binary data
+ /// The byte array will be empty when the digital status has not changed
+ public override byte[] GenerateBinary() {
+ if (!stateUpdated)
+ return Array.Empty();
+
+ byte[] bytes = new byte[1];
+ bytes[0] = (byte)(state ? 1 : 0);
+ stateUpdated = false;
+ return bytes;
+ }
+
+ ///
+ /// Function used to process binary data received for this digital sensor
+ ///
+ /// The binary data to process
+ public override void ProcessBinary(byte[] bytes) {
+ this.state |= (bytes[0] == 1);
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/Things/TemperatureSensor.cs b/src/Things/TemperatureSensor.cs
index aa3632a..6475d06 100644
--- a/src/Things/TemperatureSensor.cs
+++ b/src/Things/TemperatureSensor.cs
@@ -1,4 +1,4 @@
-//using System;
+using System;
namespace RoboidControl {
@@ -6,27 +6,50 @@ namespace RoboidControl {
/// A temperature sensor
///
public class TemperatureSensor : Thing {
+ ///
+ /// Create a temperature sensor without communication abilities
+ ///
+ /// Invoke a OnNewThing event when the thing has been created
+ public TemperatureSensor(bool invokeEvent = true) : base(Type.TemperatureSensor, invokeEvent) { }
+ ///
+ /// Create a temperature sensor for a participant
+ ///
+ /// The participant for with the sensor is needed
+ /// The ID of the thing
+ /// Invoke a OnNewThing event when the thing has been created
+ public TemperatureSensor(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.TemperatureSensor, thingId, invokeEvent) { }
+
+ ///
+ /// Create a new child temperature sensor
+ ///
+ /// 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 TemperatureSensor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.TemperatureSensor, thingId, invokeEvent) { }
+
///
/// The measured temperature
///
public float temperature = 0;
///
- /// Create a temperature sensor with the given ID
+ /// Function used to generate binary data for this temperature sensor
///
- /// The participant for with the sensor is needed
- /// The network ID of the sensor
- /// The ID of the thing
- public TemperatureSensor(Participant participant, byte thingId) : base(participant, Type.TemperatureSensor, thingId) { }
+ /// A byte array with the binary data
+ public override byte[] GenerateBinary() {
+ byte[] bytes = new byte[2];
+ byte ix = 0;
+ LowLevelMessages.SendFloat16(bytes, ref ix, this.temperature);
+ return bytes;
+ }
///
- /// Function to extract the temperature received in the binary message
+ /// Function to process the temperature from the binary data
///
- /// The byte array
+ /// The binary data to process
public override void ProcessBinary(byte[] bytes) {
byte ix = 0;
this.temperature = LowLevelMessages.ReceiveFloat16(bytes, ref ix);
- //Console.WriteLine($"temperature {this.name} = {this.temperature} C");
}
}
diff --git a/src/Things/TouchSensor.cs b/src/Things/TouchSensor.cs
index 93c7536..60e050f 100644
--- a/src/Things/TouchSensor.cs
+++ b/src/Things/TouchSensor.cs
@@ -6,32 +6,29 @@ namespace RoboidControl {
/// A sensor which can detect touches
///
public class TouchSensor : Thing {
-
///
- /// Create a touch sensor
+ /// Create a touch sensor without communication abilities
///
- /// The participant for with the sensor is needed
- /// True when the creation should trigger an event
- public TouchSensor(Participant owner) : base(owner, Type.TouchSensor) {
- Console.Write("TouchSensor constructor");
- //touchedSomething = false;
- //thisParticipant = owner;
- }
-
- public TouchSensor(Participant owner, byte networkId, byte thingId) : base(owner, networkId, thingId) {
- // Console.Write("TouchSensor constructor");
- //touchedSomething = false;
- //thisParticipant = participant;
- }
-
- public TouchSensor(Thing parent, bool invokeEvent = true) : base(parent, (byte)Type.TouchSensor, invokeEvent) { }
-
- public ParticipantUDP thisParticipant;
+ /// Invoke a OnNewThing event when the thing has been created
+ public TouchSensor(bool invokeEvent = true) : base(Type.TouchSensor, invokeEvent) { }
+ ///
+ /// Create a touch sensor 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 TouchSensor(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.TouchSensor, thingId, invokeEvent) { }
+ ///
+ /// Create a new child touch sensor
+ ///
+ /// 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) { }
///
/// Value which is true when the sensor is touching something, false otherwise
///
- //public bool touchedSomething = false;
private bool _touchedSomething = false;
public bool touchedSomething {
get { return _touchedSomething; }
@@ -52,14 +49,27 @@ namespace RoboidControl {
this.component.core = this;
}
#endif
+ ///
+ /// Function used to generate binary data for this touch sensor
+ ///
+ /// 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)
- return new byte[0];
+ return Array.Empty();
- byte[] buffer = new byte[1];
- buffer[0] = (byte)(touchedSomething ? 1 : 0);
+ byte[] bytes = new byte[1];
+ bytes[0] = (byte)(touchedSomething ? 1 : 0);
touchUpdated = false;
- return buffer;
+ return bytes;
+ }
+
+ ///
+ /// 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);
}
}
}
\ No newline at end of file