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