diff --git a/Examples/BB2B/BB2B.cs b/Examples/BB2B/BB2B.cs
index 38fa78f..a5251fd 100644
--- a/Examples/BB2B/BB2B.cs
+++ b/Examples/BB2B/BB2B.cs
@@ -10,8 +10,9 @@ namespace RoboidControl {
public BB2B(Participant owner) : base(owner) {
this.name = "BB2B";
- this.wheelRadius = 0.032f;
- this.wheelSeparation = 0.128f;
+ this.SetDriveDimensions(0.064f, 0.128f);
+ // this.wheelRadius = 0.032f;
+ // this.wheelSeparation = 0.128f;
// Is has a touch sensor at the front left of the roboid
touchLeft = new(this) {
@@ -28,11 +29,11 @@ namespace RoboidControl {
// 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;
+ float leftWheelSpeed = touchRight.touchedSomething ? -0.1f : 0.1f;
// The right wheel does the same, but instead is controlled by
// touches on the left side
- float rightWheelSpeed = touchLeft.touchedSomething ? -600.0f : 600.0f;
+ float rightWheelSpeed = touchLeft.touchedSomething ? -0.1f : 0.1f;
// When both sides are touching something, both wheels will turn backward
// and the roboid will move backwards
diff --git a/Unity/DifferentialDrive.cs b/Unity/DifferentialDrive.cs
index 3e669ad..5977740 100644
--- a/Unity/DifferentialDrive.cs
+++ b/Unity/DifferentialDrive.cs
@@ -1,12 +1,11 @@
#if UNITY_5_3_OR_NEWER
-using System.Linq;
using UnityEngine;
namespace RoboidControl.Unity {
public class DifferentialDrive : Thing {
- public WheelCollider leftWheel;
- public WheelCollider rightWheel;
+ public Wheel leftWheel;
+ public Wheel rightWheel;
///
/// Create the Unity representation
@@ -15,57 +14,64 @@ namespace RoboidControl.Unity {
/// The Unity representation of the touch sensor
public static DifferentialDrive Create(RoboidControl.DifferentialDrive core) {
GameObject prefab = (GameObject)Resources.Load("DifferentialDrive");
- if (prefab != null) {
- // Use resource prefab when available
- GameObject gameObj = Instantiate(prefab);
- DifferentialDrive component = gameObj.GetComponent();
- if (component != null)
- component.core = core;
- return component;
- }
- else {
- // Fallback implementation
- GameObject gameObj = new(core.name);
- DifferentialDrive component = gameObj.AddComponent();
- component.Init(core);
+ // if (prefab != null) {
+ // // Use resource prefab when available
+ // GameObject gameObj = Instantiate(prefab);
+ // DifferentialDrive component = gameObj.GetComponent();
+ // if (component != null)
+ // component.core = core;
+ // return component;
+ // }
+ // else {
+ // Fallback implementation
+ 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;
- }
+ Rigidbody rb = gameObj.AddComponent();
+ rb.isKinematic = false;
+ rb.mass = 0.5f;
+ return component;
+ // }
}
protected override void HandleBinary() {
+ Debug.Log("Diff drive handle Binary");
RoboidControl.DifferentialDrive coreDrive = core as RoboidControl.DifferentialDrive;
+ RoboidControl.Unity.Wheel[] motors = null;
+ if (coreDrive.wheelRadius <= 0 || coreDrive.wheelSeparation <= 0)
+ return;
+
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 = 100f, // Very high spring value to make it rigid
- damper = 10f, // Low damping (could be adjusted for slight 'bounciness')
- targetPosition = 0.5f // Neutral position (middle of the suspension travel)
- };
+ motors = GetComponentsInChildren();
+ foreach (var motor in motors) {
+ if (motor.core.id == coreDrive.leftWheel.id)
+ leftWheel = motor;
+ }
+ if (leftWheel == null)
+ leftWheel = Wheel.Create(this.GetComponent(), coreDrive.leftWheel.id);
}
- leftWheel.radius = coreDrive.wheelRadius;
- leftWheel.center = new Vector3(-coreDrive.wheelSeparation / 2, 0, 0);
+ if (leftWheel != null) {
+ leftWheel.wheelCollider.radius = coreDrive.wheelRadius;
+ leftWheel.wheelCollider.center = new Vector3(-coreDrive.wheelSeparation / 2, 0, 0);
+ leftWheel.transform.position = Vector3.zero; // position is done with the center, but only X direction is supported right now...
+ }
+
if (rightWheel == null) {
- GameObject rightWheelObj = new GameObject("Right wheel");
- rightWheelObj.transform.SetParent(this.transform);
- rightWheel = rightWheelObj.AddComponent();
- rightWheel.mass = 0.1f;
- rightWheel.suspensionDistance = 0.01f;
- rightWheel.suspensionSpring = new JointSpring {
- spring = 100f, // Very high spring value to make it rigid
- damper = 10f, // Low damping (could be adjusted for slight 'bounciness')
- targetPosition = 0.5f // Neutral position (middle of the suspension travel)
- };
+ if (motors == null)
+ motors = GetComponentsInChildren();
+ foreach (var motor in motors) {
+ if (motor.core.id == coreDrive.rightWheel.id)
+ rightWheel = motor;
+ }
+ if (rightWheel == null)
+ rightWheel = Wheel.Create(this.GetComponent(), coreDrive.rightWheel.id);
+ }
+ if (rightWheel != null && coreDrive.wheelRadius > 0 && coreDrive.wheelSeparation > 0) {
+ rightWheel.wheelCollider.radius = coreDrive.wheelRadius;
+ rightWheel.wheelCollider.center = new Vector3(coreDrive.wheelSeparation / 2, 0, 0);
+ rightWheel.transform.position = Vector3.zero; // position is done with the center, but only X direction is supported right now...
}
- rightWheel.radius = coreDrive.wheelRadius;
- rightWheel.center = new Vector3(coreDrive.wheelSeparation / 2, 0, 0);
}
}
diff --git a/Unity/Motor.cs b/Unity/Motor.cs
new file mode 100644
index 0000000..6b2b4f3
--- /dev/null
+++ b/Unity/Motor.cs
@@ -0,0 +1,49 @@
+#if UNITY_5_3_OR_NEWER
+using UnityEngine;
+
+namespace RoboidControl.Unity {
+ public class Motor : Thing {
+ ///
+ /// Create the Unity representation
+ ///
+ /// The core motor
+ /// The Unity representation of a motor
+ public static Motor Create(RoboidControl.Motor core) {
+ GameObject prefab = (GameObject)Resources.Load("Motor");
+ if (prefab != null) {
+ // Use resource prefab when available
+ GameObject gameObj = Instantiate(prefab);
+ Motor component = gameObj.GetComponent();
+ if (component != null)
+ component.core = core;
+ return component;
+ }
+ else {
+ // Fallback implementation
+ GameObject gameObj = new(core.name);
+ Motor component = gameObj.AddComponent();
+ component.Init(core);
+
+ Rigidbody rb = gameObj.AddComponent();
+ rb.isKinematic = true;
+ return component;
+ }
+ }
+
+ public float rotationSpeed = 0.0f;
+
+ protected override void HandleBinary() {
+ RoboidControl.Motor coreMotor = core as RoboidControl.Motor;
+ this.rotationSpeed = coreMotor.targetSpeed;
+ }
+
+ protected override void Update() {
+ base.Update();
+ // We rotate the first child of the motor, which should be the axle.
+ if (this.transform.childCount > 0) {
+ this.transform.GetChild(0).Rotate(360 * this.rotationSpeed, 0, 0);
+ }
+ }
+ }
+}
+#endif
\ No newline at end of file
diff --git a/Unity/Participant.cs b/Unity/Participant.cs
index c51aadb..af73e1d 100644
--- a/Unity/Participant.cs
+++ b/Unity/Participant.cs
@@ -34,6 +34,12 @@ 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.Thing coreThing:
if (coreThing.component != null) {
Thing thing = Thing.Create(coreThing);
diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs
index 76dd34c..3226edc 100644
--- a/Unity/SiteServer.cs
+++ b/Unity/SiteServer.cs
@@ -7,14 +7,15 @@ namespace RoboidControl.Unity {
public class SiteServer : Participant {
- public RoboidControl.SiteServer site;
+ //public RoboidControl.SiteServer site;
+ public RoboidControl.SiteServer site => this.coreParticipant as RoboidControl.SiteServer;
- public Queue thingQueue = new();
+ //public Queue thingQueue = new();
protected virtual void Awake() {
Console.SetOut(new UnityLogWriter());
- site = new RoboidControl.SiteServer(port);
+ this.coreParticipant = new RoboidControl.SiteServer(port);
}
void OnApplicationQuit() {
@@ -26,12 +27,12 @@ namespace RoboidControl.Unity {
if (site == null)
return;
- if (site.updateQueue.TryDequeue(out RoboidControl.Participant.UpdateEvent e))
+ while (site.updateQueue.TryDequeue(out RoboidControl.Participant.UpdateEvent e))
HandleUpdateEvent(e);
site.Update((ulong)(Time.time * 1000));
- while (thingQueue.TryDequeue(out RoboidControl.Thing thing))
- thing.CreateComponent();
+ // while (thingQueue.TryDequeue(out RoboidControl.Thing thing))
+ // thing.CreateComponent();
}
private void HandleUpdateEvent(RoboidControl.Participant.UpdateEvent e) {
@@ -43,7 +44,6 @@ namespace RoboidControl.Unity {
break;
case ThingMsg.id:
HandleThingEvent(e);
- //e.thing.CreateComponent();
break;
}
}
diff --git a/Unity/Thing.cs b/Unity/Thing.cs
index 7a38bd4..5369589 100644
--- a/Unity/Thing.cs
+++ b/Unity/Thing.cs
@@ -47,10 +47,13 @@ namespace RoboidControl.Unity {
protected void Init(RoboidControl.Thing core) {
this.core = core;
+ this.core.component = this;
this.participant = FindAnyObjectByType();
core.owner = this.participant.coreParticipant;
- if (core.parent != null && core.parent.component != null)
+ if (core.parent != null && core.parent.component != null) {
this.transform.SetParent(core.parent.component.transform, false);
+ this.transform.localPosition = Vector3.zero;
+ }
if (core.position != null)
this.transform.localPosition = core.position.ToVector3();
@@ -62,14 +65,7 @@ namespace RoboidControl.Unity {
/// Update the Unity representation
///
protected virtual void Update() {
- if (core == null) {
- // Debug.Log("Core thing is gone, self destruct in 0 seconds...");
- Destroy(this);
- return;
- }
-
- if (core.updateQueue.TryDequeue(out RoboidControl.Thing.UpdateEvent e))
- HandleUpdateEvent(e);
+ UpdateThing();
if (core.linearVelocity != null && core.linearVelocity.distance != 0) {
Vector3 direction = Quaternion.AngleAxis(core.linearVelocity.direction.horizontal, Vector3.up) * Vector3.forward;
@@ -82,6 +78,17 @@ namespace RoboidControl.Unity {
}
}
+ public void UpdateThing() {
+ if (core == null) {
+ // Debug.Log("Core thing is gone, self destruct in 0 seconds...");
+ Destroy(this);
+ return;
+ }
+
+ if (core.updateQueue.TryDequeue(out RoboidControl.Thing.UpdateEvent e))
+ HandleUpdateEvent(e);
+ }
+
private void HandleUpdateEvent(RoboidControl.Thing.UpdateEvent e) {
switch (e.messageId) {
case ThingMsg.id:
@@ -96,15 +103,12 @@ namespace RoboidControl.Unity {
case ModelUrlMsg.Id:
string extension = core.modelUrl[core.modelUrl.LastIndexOf(".")..];
if (extension == ".jpg" || extension == ".png")
- StartCoroutine(LoadJPG());
+ StartCoroutine(LoadJPG());
this.modelUrl = core.modelUrl;
break;
case PoseMsg.Id:
- if (core.linearVelocity.distance == 0)
- this.transform.localPosition = core.position.ToVector3();
- if (core.angularVelocity.distance == 0)
- this.transform.localRotation = core.orientation.ToQuaternion();
+ this.HandlePose();
break;
case BinaryMsg.Id:
this.HandleBinary();
@@ -112,14 +116,6 @@ namespace RoboidControl.Unity {
}
}
- private void PoseChanged() {
- //Debug.Log($"{this} pose changed");
- if (core.positionUpdated)
- this.transform.localPosition = core.position.ToVector3();
- if (core.orientationUpdated)
- this.transform.localRotation = core.orientation.ToQuaternion();
- }
-
private IEnumerator LoadJPG() {
UnityWebRequest request = UnityWebRequestTexture.GetTexture(core.modelUrl);
yield return request.SendWebRequest();
@@ -147,7 +143,14 @@ namespace RoboidControl.Unity {
}
}
- protected virtual void HandleBinary() {}
+ protected virtual void HandlePose() {
+ if (core.linearVelocity.distance == 0)
+ this.transform.localPosition = core.position.ToVector3();
+ if (core.angularVelocity.distance == 0)
+ this.transform.localRotation = core.orientation.ToQuaternion();
+
+ }
+ protected virtual void HandleBinary() { }
}
diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs
index 63711f7..a42cfe7 100644
--- a/Unity/TouchSensor.cs
+++ b/Unity/TouchSensor.cs
@@ -13,7 +13,7 @@ namespace RoboidControl.Unity {
/// The core touch sensor
///
public RoboidControl.TouchSensor coreSensor {
- get => (RoboidControl.TouchSensor)base.core;
+ get => base.core as RoboidControl.TouchSensor;
set => base.core = value;
}
@@ -73,7 +73,8 @@ namespace RoboidControl.Unity {
}
Debug.Log($"*** {this} Touch");
- this.coreSensor.touchedSomething = true;
+ this.coreSensor.touchedSomething = true;
+ this.core.updateQueue.Enqueue(new RoboidControl.Thing.UpdateEvent(BinaryMsg.Id));
}
private void OnTriggerExit(Collider other) {
if (other.isTrigger)
diff --git a/Unity/Wheel.cs b/Unity/Wheel.cs
new file mode 100644
index 0000000..530737c
--- /dev/null
+++ b/Unity/Wheel.cs
@@ -0,0 +1,96 @@
+#if UNITY_5_3_OR_NEWER
+using UnityEngine;
+
+namespace RoboidControl.Unity {
+ public class Wheel : Motor {
+ ///
+ /// Create the Unity representation
+ ///
+ /// The core motor
+ /// The Unity representation of a motorised wheel
+ public static Wheel Create(RoboidControl.Motor core, float wheelRadius) {
+ GameObject prefab = (GameObject)Resources.Load("Wheel");
+ if (prefab != null) {
+ // Use resource prefab when available
+ GameObject gameObj = Instantiate(prefab);
+ Wheel component = gameObj.GetComponent();
+ if (component != null)
+ component.core = core;
+ return component;
+ }
+ else {
+ // Fallback implementation
+ GameObject gameObj = new(core.name);
+ Wheel component = gameObj.AddComponent();
+ component.Init(core);
+ component.wheelCollider = gameObj.AddComponent();
+ component.wheelCollider.mass = 0.1f;
+ component.wheelCollider.suspensionDistance = 0.01f;
+ component.wheelCollider.suspensionSpring = new JointSpring {
+ spring = 100f, // Very high spring value to make it rigid
+ damper = 10f, // Low damping (could be adjusted for slight 'bounciness')
+ targetPosition = 0.5f // Neutral position (middle of the suspension travel)
+ };
+ Debug.Log("Create " + core.name);
+ return component;
+ }
+ }
+ public static Wheel Create(Rigidbody rb, byte thingId) {
+ GameObject prefab = (GameObject)Resources.Load("Wheel");
+ if (prefab != null) {
+ // Use resource prefab when available
+ GameObject gameObj = Instantiate(prefab);
+ Wheel component = gameObj.GetComponent();
+ if (component != null)
+ component.core = new RoboidControl.Thing(RoboidControl.Thing.Type.UncontrolledMotor, false);
+ return component;
+ }
+ else {
+ // Fallback implementation
+ GameObject gameObj = new("Wheel");
+ gameObj.transform.parent = rb.transform;
+ Wheel component = gameObj.AddComponent();
+ SiteServer participant = FindAnyObjectByType();
+ RoboidControl.Thing core = participant.coreParticipant.Get(thingId);
+ if (core == null)
+ core = new(participant.coreParticipant, RoboidControl.Thing.Type.UncontrolledMotor, thingId, false);
+ else {
+;
+ }
+ component.Init(core);
+ component.wheelCollider = gameObj.AddComponent();
+ component.wheelCollider.mass = 0.1f;
+ component.wheelCollider.suspensionDistance = 0.01f;
+ component.wheelCollider.suspensionSpring = new JointSpring {
+ spring = 100f, // Very high spring value to make it rigid
+ damper = 10f, // Low damping (could be adjusted for slight 'bounciness')
+ targetPosition = 0.5f // Neutral position (middle of the suspension travel)
+ };
+ Debug.Log("Create placeholder Wheel ");
+ return component;
+ }
+ }
+
+ public WheelCollider wheelCollider;
+
+ protected override void HandlePose() {
+ this.wheelCollider.center = core.position.ToVector3();
+ this.transform.position = Vector3.zero; // position is done with the center
+ }
+
+ protected override void Update() {
+ UpdateThing();
+
+ if (wheelCollider.radius > 0) {
+ float targetRotationSpeed = this.rotationSpeed * 2 * Mathf.PI; // 1 rotation per second in radians
+
+ // Calculate the required motor torque
+ float requiredTorque = (targetRotationSpeed * wheelCollider.mass) / wheelCollider.radius;
+
+ // Set the motor torque
+ wheelCollider.motorTorque = requiredTorque;
+ }
+ }
+ }
+}
+#endif
\ No newline at end of file
diff --git a/src/Messages/BinaryMsg.cs b/src/Messages/BinaryMsg.cs
index d2c0570..c46fa02 100644
--- a/src/Messages/BinaryMsg.cs
+++ b/src/Messages/BinaryMsg.cs
@@ -35,8 +35,19 @@ namespace RoboidControl {
///
/// Create an empty message for sending
///
- /// The netowork ID of the thing
- /// The ID of the thing
+ /// The thing sending the binary message
+ public BinaryMsg(Thing thing) : base() {
+ this.networkId = thing.owner.networkId;
+ this.thingId = thing.id;
+ this.thing = thing;
+ this.data = this.thing.GenerateBinary();
+ this.dataLength = (byte)this.data.Length;
+ }
+ ///
+ /// Create an empty message for sending
+ ///
+ /// The network ID of the thing
+ /// The thing sending the binary message
public BinaryMsg(byte networkId, Thing thing) : base() {
this.networkId = networkId;
this.thingId = thing.id;
diff --git a/src/Things/DifferentialDrive.cs b/src/Things/DifferentialDrive.cs
index c9db363..517b108 100644
--- a/src/Things/DifferentialDrive.cs
+++ b/src/Things/DifferentialDrive.cs
@@ -18,10 +18,10 @@ 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
public DifferentialDrive(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.DifferentialDrive, thingId, invokeEvent) {
- Thing leftWheel = new(this) {
+ Motor leftWheel = new(this) {
name = "Left Wheel"
};
- Thing rightWheel = new(this) {
+ Motor rightWheel = new(this) {
name = "Right Wheel"
};
SetMotors(leftWheel, rightWheel);
@@ -46,8 +46,8 @@ namespace RoboidControl {
/// linear and angular velocity.
/// @sa SetLinearVelocity SetAngularVelocity
public void SetDriveDimensions(float wheelDiameter, float wheelSeparation) {
- this.wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
- this.wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -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;
@@ -60,7 +60,7 @@ namespace RoboidControl {
/// @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(Motor leftWheel, Motor rightWheel) {
float distance = this.wheelSeparation / 2;
this.leftWheel = leftWheel;
@@ -78,10 +78,14 @@ namespace RoboidControl {
/// @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) {
- if (this.leftWheel != null)
- this.leftWheel.angularVelocity = new Spherical(speedLeft, Direction.left);
- if (this.rightWheel != null)
- this.rightWheel.angularVelocity = new Spherical(speedRight, Direction.right);
+ if (this.leftWheel != null) {
+ this.leftWheel.targetSpeed = speedLeft;
+ //this.leftWheel.angularVelocity = new Spherical(speedLeft, Direction.left);
+ }
+ if (this.rightWheel != null) {
+ this.rightWheel.targetSpeed = speedRight;
+ //this.rightWheel.angularVelocity = new Spherical(speedRight, Direction.right);
+ }
}
/// @copydoc RoboidControl::Thing::Update(unsigned long)
@@ -104,17 +108,19 @@ namespace RoboidControl {
}
/// @brief The radius of a wheel in meters
- public float wheelRadius = 1.0f;
+ private float _wheelRadius = 0.0f;
+ public float wheelRadius { get => _wheelRadius; }
/// @brief The distance between the wheels in meters
- public float wheelSeparation = 1.0f;
+ private float _wheelSeparation = 0.0f;
+ public float wheelSeparation { get => _wheelSeparation; }
/// @brief Convert revolutions per second to meters per second
protected float rpsToMs = 1.0f;
/// @brief The left wheel
- public Thing leftWheel = null;
+ public Motor leftWheel = null;
/// @brief The right wheel
- public Thing rightWheel = null;
+ public Motor rightWheel = null;
bool sendBinary = false;
public override byte[] GenerateBinary() {
@@ -134,11 +140,11 @@ namespace RoboidControl {
public override void ProcessBinary(byte[] data) {
byte ix = 0;
byte leftWheelId = data[ix++];
- this.leftWheel = this.owner.Get(leftWheelId);
+ this.leftWheel = this.owner.Get(leftWheelId) as Motor;
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.rightWheel = this.owner.Get(rightWheelId) as Motor;
+ this._wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix);
+ this._wheelSeparation = LowLevelMessages.ReceiveFloat16(data, ref ix);
this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id));
}
};
diff --git a/src/Things/Motor.cs b/src/Things/Motor.cs
new file mode 100644
index 0000000..9b1d6c7
--- /dev/null
+++ b/src/Things/Motor.cs
@@ -0,0 +1,45 @@
+using LinearAlgebra;
+
+namespace RoboidControl {
+
+ public class Motor : Thing {
+ public Motor(bool invokeEvent = true) : base(Type.UncontrolledMotor, invokeEvent) { }
+ public Motor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.UncontrolledMotor, thingId, invokeEvent) { }
+
+ /// @brief Motor turning direction
+ public enum Direction {
+ Clockwise = 1,
+ CounterClockwise = -1
+ };
+ /// @brief The forward turning direction of the motor
+ public Direction direction = Direction.Clockwise;
+
+ protected float currentTargetSpeed = 0;
+
+ private float _targetSpeed;
+ ///
+ /// The speed between -1 (full reverse), 0 (stop) and 1 (full forward)
+ ///
+ public float targetSpeed {
+ get => _targetSpeed;
+ set {
+ if (value != _targetSpeed) {
+ _targetSpeed = Float.Clamp(value, -1, 1);
+ updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id));
+ owner.Send(new BinaryMsg(this));
+ }
+ }
+ }
+
+ public override byte[] GenerateBinary() {
+ byte[] data = new byte[1];
+ byte ix = 0;
+ data[ix++] = (byte)(this.targetSpeed * 127);
+ return data;
+ }
+ public override void ProcessBinary(byte[] data) {
+ this.targetSpeed = (float)data[0] / 127;
+ }
+ }
+
+}
\ No newline at end of file