diff --git a/Examples/BB2B/BB2B.cs b/Examples/BB2B/BB2B.cs index 4f1c3cf..2a398ca 100644 --- a/Examples/BB2B/BB2B.cs +++ b/Examples/BB2B/BB2B.cs @@ -9,7 +9,7 @@ namespace RoboidControl { readonly TouchSensor touchRight; const float speed = 0.5f; - public BB2B(Participant owner) : base(owner) { + public BB2B(Thing parent = default) : base(parent) { this.name = "BB2B"; this.SetMotors(new Motor(this), new Motor(this)); this.SetDriveDimensions(0.064f, 0.128f); diff --git a/Examples/BB2B/BB2B_Encoder.cs b/Examples/BB2B/BB2B_Encoder.cs index f2b91df..21787c6 100644 --- a/Examples/BB2B/BB2B_Encoder.cs +++ b/Examples/BB2B/BB2B_Encoder.cs @@ -9,15 +9,15 @@ namespace RoboidControl { readonly TouchSensor touchRight; const float speed = 180.0f; // wheel rotation speed in degrees - public BB2B_Encoder(Participant owner) : base(owner) { + public BB2B_Encoder(Thing parent) : base(parent) { this.name = "BB2B"; this.SetDriveDimensions(0.064f, 0.128f); // Update the basic motors to motors with encoder - EncoderMotor leftMotor = new(this, new RelativeEncoder()) { + ControlledMotor leftMotor = new(this, new RelativeEncoder()) { position = new Spherical(0.064f, Direction.left) }; - EncoderMotor rightMotor = new(this, new RelativeEncoder()) { + ControlledMotor rightMotor = new(this, new RelativeEncoder()) { position = new Spherical(0.064f, Direction.right) }; this.SetMotors(leftMotor, rightMotor); diff --git a/Examples/BB2B/Program.cs b/Examples/BB2B/Program.cs index 10b0c67..7550097 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(ParticipantUDP.Isolated()); + BB2B bb2b = new(); while (true) { bb2b.Update(); diff --git a/LinearAlgebra/src/Angle.cs b/LinearAlgebra/src/Angle.cs index f056e5a..1490983 100644 --- a/LinearAlgebra/src/Angle.cs +++ b/LinearAlgebra/src/Angle.cs @@ -1,17 +1,128 @@ using System; +using System.Reflection.Emit; namespace LinearAlgebra { - /// - /// %Angle utilities - /// - public static class Angle { - public const float pi = 3.1415927410125732421875F; - // public static float Rad2Deg = 360.0f / ((float)Math.PI * 2); - // public static float Deg2Rad = ((float)Math.PI * 2) / 360.0f; - + public class Angle { public const float Rad2Deg = 360.0f / ((float)Math.PI * 2); //0.0174532924F; - public const float Deg2Rad = ((float)Math.PI * 2) / 360.0f; //57.29578F; + public const float Deg2Rad = (float)Math.PI * 2 / 360.0f; //57.29578F; + + private Angle(float degrees) { + this.value = degrees; + } + private readonly float value = 0; + + public static readonly Angle zero = new(0); + + public static Angle Degrees(float degrees) { + // Reduce it to (-180..180] + if (float.IsFinite(degrees)) { + while (degrees < -180) + degrees += 360; + while (degrees >= 180) + degrees -= 360; + } + return new Angle(degrees); + } + + public static Angle Radians(float radians) { + // Reduce it to (-pi..pi] + if (float.IsFinite(radians)) { + while (radians <= -Math.PI) + radians += 2 * (float)Math.PI; + while (radians > Math.PI) + radians -= 2 * (float)Math.PI; + } + + return new Angle(radians * Rad2Deg); + + } + + public static Angle Revolutions(float revolutions) { + // reduce it to (-0.5 .. 0.5] + if (float.IsFinite(revolutions)) { + // Get the integer part + int integerPart = (int)revolutions; + + // Get the decimal part + revolutions -= integerPart; + if (revolutions < -0.5) + revolutions += 1; + if (revolutions >= 0.5) + revolutions -= 1; + } + return new Angle(revolutions * 360); + } + + public float inDegrees { + get { return this.value; } + } + + public float inRadians { + get { return this.value * Deg2Rad; } + } + + public float inRevolutions { + get { return this.value / 360.0f; } + } + + /// + /// Get the sign of the angle + /// + /// The angle + /// -1 when the angle is negative, 1 when it is positive and 0 in all other cases + public static int Sign(Angle a) { + if (a.value < 0) + return -1; + if (a.value > 0) + return 1; + return 0; + } + + /// + /// Returns the magnitude of the angle + /// + /// The angle + /// The positive magnitude of the angle + /// Negative values are negated to get a positive result + public static Angle Abs(Angle a) { + if (Sign(a) < 0) + return -a; + else + return a; + } + + /// + /// Negate the angle + /// + /// The angle + /// The negated angle + /// The negation of -180 is still -180 because the range is (-180..180] + public static Angle operator -(Angle a) { + Angle r = new(-a.value); + return r; + } + + /// + /// Subtract two angles + /// + /// Angle 1 + /// Angle 2 + /// The result of the subtraction + public static Angle operator -(Angle a1, Angle a2) { + Angle r = new(a1.value - a2.value); + return r; + } + /// + /// Add two angles + /// + /// Angle 1 + /// Angle 2 + /// The result of the addition + public static Angle operator +(Angle a1, Angle a2) { + Angle r = new(a1.value + a2.value); + return r; + } /// /// Clamp the angle between the given min and max values @@ -21,11 +132,38 @@ namespace LinearAlgebra { /// The maximum angle /// The clamped angle /// Angles are normalized - public static float Clamp(float angle, float min, float max) { - float normalizedAngle = Normalize(angle); - return Float.Clamp(normalizedAngle, min, max); + public static float Clamp(Angle angle, Angle min, Angle max) { + return Float.Clamp(angle.inDegrees, min.inDegrees, max.inDegrees); } + /// + /// Rotate from one angle to the other with a maximum degrees + /// + /// Starting angle + /// Target angle + /// Maximum angle to rotate + /// The resulting angle + /// This function is compatible with radian and degrees angles + public static Angle MoveTowards(Angle fromAngle, Angle toAngle, float maxDegrees) { + maxDegrees = Math.Max(0, maxDegrees); // filter out negative distances + Angle d = toAngle - fromAngle; + float dDegrees = Abs(d).inDegrees; + d = Degrees(Float.Clamp(dDegrees, 0, maxDegrees)); + if (Sign(d) < 0) + d = -d; + return fromAngle + d; + } + } + + /// + /// %Angle utilities + /// + public static class Angles { + public const float pi = 3.1415927410125732421875F; + // public static float Rad2Deg = 360.0f / ((float)Math.PI * 2); + // public static float Deg2Rad = ((float)Math.PI * 2) / 360.0f; + + /// /// Determine the angle difference, result is a normalized angle /// @@ -53,21 +191,6 @@ namespace LinearAlgebra { return angle; } - /// - /// Rotate from one angle to the other with a maximum degrees - /// - /// Starting angle - /// Target angle - /// Maximum angle to rotate - /// The resulting angle - /// This function is compatible with radian and degrees angles - public static float MoveTowards(float fromAngle, float toAngle, float maxAngle) { - float d = toAngle - fromAngle; - d = Normalize(d); - d = Math.Sign(d) * Float.Clamp(Math.Abs(d), 0, maxAngle); - return fromAngle + d; - } - /// /// Map interval of angles between vectors [0..Pi] to interval [0..1] /// diff --git a/LinearAlgebra/src/Direction.cs b/LinearAlgebra/src/Direction.cs index 6039bd5..297eff9 100644 --- a/LinearAlgebra/src/Direction.cs +++ b/LinearAlgebra/src/Direction.cs @@ -25,6 +25,10 @@ namespace LinearAlgebra horizontal = 0; vertical = 0; } + public Direction(Angle horizontal, Angle vertical) { + this.horizontal = horizontal.inDegrees; + + } // public Direction(float horizontal, float vertical) { // this.horizontal = horizontal; // this.vertical = vertical; @@ -64,7 +68,7 @@ namespace LinearAlgebra public Vector3Float ToVector3() { - float verticalRad = (Angle.pi / 2) - this.vertical * Angle.Deg2Rad; + float verticalRad = ((float)Math.PI / 2) - this.vertical * Angle.Deg2Rad; float horizontalRad = this.horizontal * Angle.Deg2Rad; float cosVertical = (float)Math.Cos(verticalRad); float sinVertical = (float)Math.Sin(verticalRad); diff --git a/LinearAlgebra/src/Spherical.cs b/LinearAlgebra/src/Spherical.cs index 456646f..183aa01 100644 --- a/LinearAlgebra/src/Spherical.cs +++ b/LinearAlgebra/src/Spherical.cs @@ -83,7 +83,7 @@ namespace LinearAlgebra { if (distance == 0.0f) return Spherical.zero; else { - float verticalAngle = (float)((Angle.pi / 2 - Math.Acos(v.y / distance)) * Angle.Rad2Deg); + float verticalAngle = (float)(Math.PI / 2 - Math.Acos(v.y / distance)) * Angle.Rad2Deg; float horizontalAngle = (float)Math.Atan2(v.x, v.z) * Angle.Rad2Deg; return Spherical.Degrees(distance, horizontalAngle, verticalAngle); } @@ -106,7 +106,7 @@ namespace LinearAlgebra { // } public Vector3 ToVector3() { - float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad; + float verticalRad = (float)(Math.PI / 2 - this.direction.vertical) * Angle.Deg2Rad; float horizontalRad = this.direction.horizontal * Angle.Deg2Rad; float cosVertical = (float)Math.Cos(verticalRad); float sinVertical = (float)Math.Sin(verticalRad); diff --git a/LinearAlgebra/src/SwingTwist.cs b/LinearAlgebra/src/SwingTwist.cs index 22eb0bb..58c1a1a 100644 --- a/LinearAlgebra/src/SwingTwist.cs +++ b/LinearAlgebra/src/SwingTwist.cs @@ -29,11 +29,18 @@ namespace LinearAlgebra { } #if UNITY_5_3_OR_NEWER + public static SwingTwist FromQuaternion(Quaternion q) { + // q.ToAngles(out float right, out float up, out float forward); + UnityEngine.Vector3 angles = q.eulerAngles; + SwingTwist r = new SwingTwist(angles.y, angles.x, angles.z); + return r; + } + public Quaternion ToQuaternion() { Quaternion q = Quaternion.Euler(-this.swing.vertical, this.swing.horizontal, this.twist); - return q; + return q; } #endif } diff --git a/LinearAlgebra/test/AngleTest.cs b/LinearAlgebra/test/AngleTest.cs index c248465..00e4981 100644 --- a/LinearAlgebra/test/AngleTest.cs +++ b/LinearAlgebra/test/AngleTest.cs @@ -1,169 +1,207 @@ #if !UNITY_5_6_OR_NEWER +using System; +using System.Formats.Asn1; using NUnit.Framework; -namespace LinearAlgebra.Test -{ - public class Tests - { +namespace LinearAlgebra.Test { + public class AngleTests { [SetUp] - public void Setup() - { + public void Setup() { } [Test] - public void Normalize() - { - float r = 0; + public void Construct() { + // Degrees + float angle = 0.0f; + Angle a = Angle.Degrees(angle); + Assert.AreEqual(angle, a.inDegrees); - r = Angle.Normalize(90); - Assert.AreEqual(r, 90, "Normalize 90"); + angle = -180.0f; + a = Angle.Degrees(angle); + Assert.AreEqual(angle, a.inDegrees); - r = Angle.Normalize(-90); - Assert.AreEqual(r, -90, "Normalize -90"); + angle = 270.0f; + a = Angle.Degrees(angle); + Assert.AreEqual(-90, a.inDegrees); - r = Angle.Normalize(270); - Assert.AreEqual(r, -90, "Normalize 270"); + // Radians + angle = 0.0f; + a = Angle.Radians(angle); + Assert.AreEqual(angle, a.inRadians); - r = Angle.Normalize(270 + 360); - Assert.AreEqual(r, -90, "Normalize 270+360"); + angle = (float)-Math.PI; + a = Angle.Radians(angle); + Assert.AreEqual(angle, a.inRadians); - r = Angle.Normalize(-270); - Assert.AreEqual(r, 90, "Normalize -270"); + angle = (float)Math.PI * 1.5f; + a = Angle.Radians(angle); + Assert.AreEqual(-Math.PI * 0.5f, a.inRadians); - r = Angle.Normalize(-270 - 360); - Assert.AreEqual(r, 90, "Normalize -270-360"); + // Revolutions + angle = 0.0f; + a = Angle.Revolutions(angle); + Assert.AreEqual(angle, a.inRevolutions); - r = Angle.Normalize(0); - Assert.AreEqual(r, 0, "Normalize 0"); + angle = -0.5f; + a = Angle.Revolutions(angle); + Assert.AreEqual(angle, a.inRevolutions); - r = Angle.Normalize(float.PositiveInfinity); - Assert.AreEqual(r, float.PositiveInfinity, "Normalize INFINITY"); + angle = 0.75f; + a = Angle.Revolutions(angle); + Assert.AreEqual(-0.25f, a.inRevolutions); - r = Angle.Normalize(float.NegativeInfinity); - Assert.AreEqual(r, float.NegativeInfinity, "Normalize INFINITY"); } + // [Test] + // public void Normalize() { + // float r = 0; + + // r = Angle.Normalize(90); + // Assert.AreEqual(r, 90, "Normalize 90"); + + // r = Angle.Normalize(-90); + // Assert.AreEqual(r, -90, "Normalize -90"); + + // r = Angle.Normalize(270); + // Assert.AreEqual(r, -90, "Normalize 270"); + + // r = Angle.Normalize(270 + 360); + // Assert.AreEqual(r, -90, "Normalize 270+360"); + + // r = Angle.Normalize(-270); + // Assert.AreEqual(r, 90, "Normalize -270"); + + // r = Angle.Normalize(-270 - 360); + // Assert.AreEqual(r, 90, "Normalize -270-360"); + + // r = Angle.Normalize(0); + // Assert.AreEqual(r, 0, "Normalize 0"); + + // r = Angle.Normalize(float.PositiveInfinity); + // Assert.AreEqual(r, float.PositiveInfinity, "Normalize INFINITY"); + + // r = Angle.Normalize(float.NegativeInfinity); + // Assert.AreEqual(r, float.NegativeInfinity, "Normalize INFINITY"); + // } + [Test] - public void Clamp() - { + public void Clamp() { float r = 0; - r = Angle.Clamp(1, 0, 2); + r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(2)); Assert.AreEqual(r, 1, "Clamp 1 0 2"); - r = Angle.Clamp(-1, 0, 2); + r = Angle.Clamp(Angle.Degrees(-1), Angle.Degrees(0), Angle.Degrees(2)); Assert.AreEqual(r, 0, "Clamp -1 0 2"); - r = Angle.Clamp(3, 0, 2); + r = Angle.Clamp(Angle.Degrees(3), Angle.Degrees(0), Angle.Degrees(2)); Assert.AreEqual(r, 2, "Clamp 3 0 2"); - r = Angle.Clamp(1, 0, 0); + r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(0)); Assert.AreEqual(r, 0, "Clamp 1 0 0"); - r = Angle.Clamp(0, 0, 0); + r = Angle.Clamp(Angle.Degrees(0), Angle.Degrees(0), Angle.Degrees(0)); Assert.AreEqual(r, 0, "Clamp 0 0 0"); - r = Angle.Clamp(0, 1, -1); + r = Angle.Clamp(Angle.Degrees(0), Angle.Degrees(1), Angle.Degrees(-1)); Assert.AreEqual(r, 1, "Clamp 0 1 -1"); - r = Angle.Clamp(1, 0, float.PositiveInfinity); + r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(float.PositiveInfinity)); Assert.AreEqual(r, 1, "Clamp 1 0 INFINITY"); - r = Angle.Clamp(1, float.NegativeInfinity, 1); + r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(float.NegativeInfinity), Angle.Degrees(1)); Assert.AreEqual(r, 1, "Clamp 1 -INFINITY 1"); } - [Test] - public void Difference() - { - float r = 0; + // [Test] + // public void Difference() { + // float r = 0; - r = Angle.Difference(0, 90); - Assert.AreEqual(r, 90, "Difference 0 90"); + // r = Angle.Difference(0, 90); + // Assert.AreEqual(r, 90, "Difference 0 90"); - r = Angle.Difference(0, -90); - Assert.AreEqual(r, -90, "Difference 0 -90"); + // r = Angle.Difference(0, -90); + // Assert.AreEqual(r, -90, "Difference 0 -90"); - r = Angle.Difference(0, 270); - Assert.AreEqual(r, -90, "Difference 0 270"); + // r = Angle.Difference(0, 270); + // Assert.AreEqual(r, -90, "Difference 0 270"); - r = Angle.Difference(0, -270); - Assert.AreEqual(r, 90, "Difference 0 -270"); + // r = Angle.Difference(0, -270); + // Assert.AreEqual(r, 90, "Difference 0 -270"); - r = Angle.Difference(90, 0); - Assert.AreEqual(r, -90, "Difference 90 0"); + // r = Angle.Difference(90, 0); + // Assert.AreEqual(r, -90, "Difference 90 0"); - r = Angle.Difference(-90, 0); - Assert.AreEqual(r, 90, "Difference -90 0"); + // r = Angle.Difference(-90, 0); + // Assert.AreEqual(r, 90, "Difference -90 0"); - r = Angle.Difference(0, 0); - Assert.AreEqual(r, 0, "Difference 0 0"); + // r = Angle.Difference(0, 0); + // Assert.AreEqual(r, 0, "Difference 0 0"); - r = Angle.Difference(90, 90); - Assert.AreEqual(r, 0, "Difference 90 90"); + // r = Angle.Difference(90, 90); + // Assert.AreEqual(r, 0, "Difference 90 90"); - r = Angle.Difference(0, float.PositiveInfinity); - Assert.AreEqual(r, float.PositiveInfinity, "Difference 0 INFINITY"); + // r = Angle.Difference(0, float.PositiveInfinity); + // Assert.AreEqual(r, float.PositiveInfinity, "Difference 0 INFINITY"); - r = Angle.Difference(0, float.NegativeInfinity); - Assert.AreEqual(r, float.NegativeInfinity, "Difference 0 -INFINITY"); + // r = Angle.Difference(0, float.NegativeInfinity); + // Assert.AreEqual(r, float.NegativeInfinity, "Difference 0 -INFINITY"); - r = Angle.Difference(float.NegativeInfinity, float.PositiveInfinity); - Assert.AreEqual(r, float.PositiveInfinity, "Difference -INFINITY INFINITY"); - } + // r = Angle.Difference(float.NegativeInfinity, float.PositiveInfinity); + // Assert.AreEqual(r, float.PositiveInfinity, "Difference -INFINITY INFINITY"); + // } [Test] - public void MoveTowards() - { - float r = 0; + public void MoveTowards() { + Angle r = Angle.zero; - r = Angle.MoveTowards(0, 90, 30); - Assert.AreEqual(r, 30, "MoveTowards 0 90 30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 30); + Assert.AreEqual(r.inDegrees, 30, "MoveTowards 0 90 30"); - r = Angle.MoveTowards(0, 90, 90); - Assert.AreEqual(r, 90, "MoveTowards 0 90 90"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 90); + Assert.AreEqual(r.inDegrees, 90, "MoveTowards 0 90 90"); - r = Angle.MoveTowards(0, 90, 180); - Assert.AreEqual(r, 90, "MoveTowards 0 90 180"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 180); + Assert.AreEqual(r.inDegrees, 90, "MoveTowards 0 90 180"); - r = Angle.MoveTowards(0, 90, 270); - Assert.AreEqual(r, 90, "MoveTowrads 0 90 270"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 270); + Assert.AreEqual(r.inDegrees, 90, "MoveTowrads 0 90 270"); - r = Angle.MoveTowards(0, 90, -30); - Assert.AreEqual(r, -30, "MoveTowards 0 90 -30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), -30); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 90 -30"); - r = Angle.MoveTowards(0, -90, -30); - Assert.AreEqual(r, 30, "MoveTowards 0 -90 -30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -30); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 -90 -30"); - r = Angle.MoveTowards(0, -90, -90); - Assert.AreEqual(r, 90, "MoveTowards 0 -90 -90"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -90); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 -90 -90"); - r = Angle.MoveTowards(0, -90, -180); - Assert.AreEqual(r, 180, "MoveTowards 0 -90 -180"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -180); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 -90 -180"); - r = Angle.MoveTowards(0, -90, -270); - Assert.AreEqual(r, 270, "MoveTowrads 0 -90 -270"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -270); + Assert.AreEqual(r.inDegrees, 0, "MoveTowrads 0 -90 -270"); - r = Angle.MoveTowards(0, 90, 0); - Assert.AreEqual(r, 0, "MoveTowards 0 90 0"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 0); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 90 0"); - r = Angle.MoveTowards(0, 0, 0); - Assert.AreEqual(r, 0, "MoveTowards 0 0 0"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(0), 0); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 0 0"); - r = Angle.MoveTowards(0, 0, 30); - Assert.AreEqual(r, 0, "MoveTowrads 0 0 30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(0), 30); + Assert.AreEqual(r.inDegrees, 0, "MoveTowrads 0 0 30"); - r = Angle.MoveTowards(0, 90, float.PositiveInfinity); - Assert.AreEqual(r, 90, "MoveTowards 0 90 INFINITY"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), float.PositiveInfinity); + Assert.AreEqual(r.inDegrees, 90, "MoveTowards 0 90 INFINITY"); - r = Angle.MoveTowards(0, float.PositiveInfinity, 30); - Assert.AreEqual(r, 30, "MoveTowrads 0 INFINITY 30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(float.PositiveInfinity), 30); + Assert.AreEqual(r.inDegrees, 30, "MoveTowrads 0 INFINITY 30"); - r = Angle.MoveTowards(0, -90, float.NegativeInfinity); - Assert.AreEqual(r, float.PositiveInfinity, "MoveTowards 0 -90 -INFINITY"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), float.NegativeInfinity); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 -90 -INFINITY"); - r = Angle.MoveTowards(0, float.NegativeInfinity, -30); - Assert.AreEqual(r, 30, "MoveTowrads 0 -INFINITY -30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(float.NegativeInfinity), -30); + Assert.AreEqual(r.inDegrees, 0, "MoveTowrads 0 -INFINITY -30"); } } diff --git a/LinearAlgebra/test/DirectionTest.cs b/LinearAlgebra/test/DirectionTest.cs new file mode 100644 index 0000000..3cebe1a --- /dev/null +++ b/LinearAlgebra/test/DirectionTest.cs @@ -0,0 +1,19 @@ +#if !UNITY_5_6_OR_NEWER +using NUnit.Framework; + +namespace LinearAlgebra.Test { + public class DirectionTest { + [SetUp] + public void Setup() { + } + + [Test] + public void Compare() { + Direction d = Direction.Degrees(45, 135); + bool r; + r = d == new Direction(Angle.Degrees(45), Angle.Degrees(135)); + Assert.True(r); + } + }; +} +#endif \ No newline at end of file diff --git a/LinearAlgebra/test/SphericalTest.cs b/LinearAlgebra/test/SphericalTest.cs index 3ede4f4..8539dc0 100644 --- a/LinearAlgebra/test/SphericalTest.cs +++ b/LinearAlgebra/test/SphericalTest.cs @@ -1,4 +1,5 @@ #if !UNITY_5_6_OR_NEWER +using System; using NUnit.Framework; namespace LinearAlgebra.Test { @@ -13,7 +14,19 @@ namespace LinearAlgebra.Test { Spherical s = Spherical.FromVector3(v); Assert.AreEqual(1.0f, s.distance, "s.distance 0 0 1"); Assert.AreEqual(0.0f, s.direction.horizontal, "s.hor 0 0 1"); - Assert.AreEqual(0.0f, s.direction.vertical, "s.vert 0 0 1"); + Assert.AreEqual(0.0f, s.direction.vertical, 1.0E-05F, "s.vert 0 0 1"); + + v = new(0, 1, 0); + s = Spherical.FromVector3(v); + Assert.AreEqual(1.0f, s.distance, "s.distance 0 1 0"); + Assert.AreEqual(0.0f, s.direction.horizontal, "s.hor 0 1 0"); + Assert.AreEqual(90.0f, s.direction.vertical, "s.vert 0 1 0"); + + v = new(1, 0, 0); + s = Spherical.FromVector3(v); + Assert.AreEqual(1.0f, s.distance, "s.distance 1 0 0"); + Assert.AreEqual(90.0f, s.direction.horizontal, "s.hor 1 0 0"); + Assert.AreEqual(0.0f, s.direction.vertical, 1.0E-05F, "s.vert 1 0 0"); } [Test] @@ -23,7 +36,17 @@ namespace LinearAlgebra.Test { Spherical r = Spherical.zero; r = v1 + v2; - Assert.AreEqual(v1.distance, r.distance, "Addition(0,0,0)"); + Assert.AreEqual(v1.distance, r.distance, 1.0E-05F, "Addition(0,0,0)"); + + r = v1; + r += v2; + Assert.AreEqual(v1.distance, r.distance, 1.0E-05F, "Addition(0,0,0)"); + + v2 = Spherical.Degrees(1, 0, 90); + r = v1 + v2; + Assert.AreEqual(Math.Sqrt(2), r.distance, 1.0E-05F, "Addition(1 0 90)"); + Assert.AreEqual(45.0f, r.direction.horizontal, "Addition(1 0 90)"); + Assert.AreEqual(45.0f, r.direction.vertical, "Addition(1 0 90)"); } } } diff --git a/README.md b/README.md index 7af367d..b576be8 100644 --- a/README.md +++ b/README.md @@ -11,3 +11,21 @@ The documentation for Roboid Control for C# is found at https://docs.roboidcontr - RoboidControl::Thing - RoboidControl::Participant + +# Get Started + +## Unity + +The Unity environment can use the same RoboidControl code as every other C# code, but needs a *starter* wrapper around it to make the things visibile. For example, to start the BB2B example in Unity one needs to write a BB2B_Starter.cs component as follows: +``` +using RoboidControl.Unity; + +public class BB2B_Starter : SiteServer { + void Start() { + new RoboidControl.BB2B(); + } +} +``` +This component then should be attached to a GameObject in the scene. + +It is possible to create a Site Server in Unity by just adding the `SiteServer` Component to a GameObject in the scene. When this is run, other roboids will be able to connect to this site then. \ No newline at end of file diff --git a/Unity/DifferentialDrive.cs b/Unity/DifferentialDrive.cs index 3ea68bf..4f573aa 100644 --- a/Unity/DifferentialDrive.cs +++ b/Unity/DifferentialDrive.cs @@ -42,8 +42,6 @@ namespace RoboidControl.Unity { return differentialDrive; } - public Motor leftMotor; - public Motor rightMotor; /// /// The left wheel of the differential drive /// @@ -73,8 +71,41 @@ namespace RoboidControl.Unity { /// Handle the binary event indicating a configuration change /// protected override void HandleBinary() { - HandleWheelBinary(coreDrive.leftWheel, ref leftMotor, ref leftWheel); - HandleWheelBinary(coreDrive.rightWheel, ref rightMotor, ref rightWheel); + // 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; + } if (casterWheel == null) { GameObject gameObj = new("Caster wheel"); @@ -82,30 +113,11 @@ namespace RoboidControl.Unity { casterWheel = gameObj.AddComponent(); casterWheel.material = Wheel.slidingWheel; } - 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); - } + 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); } /// @@ -114,19 +126,18 @@ namespace RoboidControl.Unity { protected override void FixedUpdate() { base.FixedUpdate(); - 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; + 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 (leftWheel != null && rightWheel != null) { - float wheelSeparation = Vector3.Distance(leftWheel.transform.position, rightWheel.transform.position); - float forwardSpeed = (leftWheelVelocity + rightWheelVelocity) / 2f; - float turningSpeed = (leftWheelVelocity - rightWheelVelocity) / wheelSeparation; + // 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; - // 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; + rb.angularVelocity = 0.9f * rb.angularVelocity + 0.1f * turningSpeed * Vector3.up; } } } diff --git a/Unity/Editor/Configurator.cs b/Unity/Editor/Configurator.cs new file mode 100644 index 0000000..25942ee --- /dev/null +++ b/Unity/Editor/Configurator.cs @@ -0,0 +1,88 @@ +using System.Collections.Generic; +using UnityEngine; +using UnityEditor; +using UnityEditor.Callbacks; +using UnityEditor.PackageManager.Requests; +using UnityEditor.PackageManager; + +namespace RoboidControl.Unity +{ + [InitializeOnLoad] + public class ConfigurationCheck + { + static ConfigurationCheck() + { + RetrievePackageList(); + } + + protected static ListRequest request; + public static List packageNameList; + + public static void RetrievePackageList() + { + request = Client.List(); // List packages installed for the Project + EditorApplication.update += Progress; + } + + public static void Progress() + { + if (request.IsCompleted) + { + if (request.Status == StatusCode.Success) + { + packageNameList = new List(); + foreach (UnityEditor.PackageManager.PackageInfo package in request.Result) + packageNameList.Add(package.name); + + DidReloadScripts(); + } + else if (request.Status >= StatusCode.Failure) + Debug.Log(request.Error.message); + + EditorApplication.update -= Progress; + } + } + + //[DidReloadScripts] + protected static void DidReloadScripts() + { + if (packageNameList == null) + return; + CheckExtension( + packageNameList.Contains("com.unity.cloud.gltfast"), "GLTF"); + } + + protected static void CheckExtension(bool enabled, string define) + { + if (enabled) + GlobalDefine(define); + else + GlobalUndefine(define); + } + + public static void GlobalDefine(string name) + { + //Debug.Log("Define " + name); + string scriptDefines = PlayerSettings.GetScriptingDefineSymbolsForGroup(EditorUserBuildSettings.selectedBuildTargetGroup); + if (!scriptDefines.Contains(name)) + { + string newScriptDefines = scriptDefines + " " + name; + if (EditorUserBuildSettings.selectedBuildTargetGroup != 0) + PlayerSettings.SetScriptingDefineSymbolsForGroup(EditorUserBuildSettings.selectedBuildTargetGroup, newScriptDefines); + } + } + + public static void GlobalUndefine(string name) + { + //Debug.Log("Undefine " + name); + string scriptDefines = PlayerSettings.GetScriptingDefineSymbolsForGroup(EditorUserBuildSettings.selectedBuildTargetGroup); + if (scriptDefines.Contains(name)) + { + int playMakerIndex = scriptDefines.IndexOf(name); + string newScriptDefines = scriptDefines.Remove(playMakerIndex, name.Length); + PlayerSettings.SetScriptingDefineSymbolsForGroup(EditorUserBuildSettings.selectedBuildTargetGroup, newScriptDefines); + } + + } + } +} \ No newline at end of file diff --git a/Unity/Motor.cs b/Unity/Motor.cs index 72cf128..3c15cdd 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; } @@ -43,7 +43,7 @@ namespace RoboidControl.Unity { /// /// The maximum rotation speed of the motor in rotations per second /// - public float maxSpeed = 2; + public float maxSpeed = 5; /// /// The actual rotation speed in rotations per second diff --git a/Unity/Participant.cs b/Unity/Participant.cs index 9a04ec5..62834a8 100644 --- a/Unity/Participant.cs +++ b/Unity/Participant.cs @@ -8,16 +8,6 @@ namespace RoboidControl.Unity { public RoboidControl.Participant coreParticipant; - // public Thing GetThing(RoboidControl.Thing coreThing) { - // // Not efficient or full correct but will do for now - // Thing[] things = FindObjectsOfType(); - // foreach (Thing thing in things) { - // if (thing.core.id == coreThing.id) - // return thing; - // } - // return null; - // } - protected virtual void Update() { if (coreParticipant == null) return; @@ -46,12 +36,12 @@ namespace RoboidControl.Unity { DifferentialDrive differentialDrive = DifferentialDrive.Create(coreDrive); coreDrive.component = differentialDrive; 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.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: Debug.Log("Handle Thing"); if (coreThing.component == null) { diff --git a/Unity/RelativeEncoder.cs b/Unity/RelativeEncoder.cs index 521878c..475a85c 100644 --- a/Unity/RelativeEncoder.cs +++ b/Unity/RelativeEncoder.cs @@ -56,7 +56,7 @@ namespace RoboidControl.Unity { // Normalize angles to range [-180..180) // Note: it is not possible to track rotation speeds higher than 180 degrees per frame because of representation limitations of Quaternions - float deltaAngle = Angle.Normalize(deltaRotation.eulerAngles.x); + float deltaAngle = Angle.Degrees(deltaRotation.eulerAngles.x).inDegrees; this.rotationSpeed = deltaAngle / Time.deltaTime; } diff --git a/Unity/Resources/DifferentialDrive_disabled.prefab b/Unity/Resources/DifferentialDrive_disabled.prefab new file mode 100644 index 0000000..3f5f12d --- /dev/null +++ b/Unity/Resources/DifferentialDrive_disabled.prefab @@ -0,0 +1,546 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!1 &135949056663158942 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 693610334404048217} + - component: {fileID: 874278397287993211} + - component: {fileID: 8149674613691108646} + - component: {fileID: 7453252590388621785} + m_Layer: 0 + m_Name: Body + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &693610334404048217 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 135949056663158942} + serializedVersion: 2 + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0.01, z: -0.01} + m_LocalScale: {x: 0.12, y: 0.05, z: 0.2} + m_ConstrainProportionsScale: 0 + m_Children: [] + m_Father: {fileID: 6798369561388671537} + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!33 &874278397287993211 +MeshFilter: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 135949056663158942} + m_Mesh: {fileID: 10202, guid: 0000000000000000e000000000000000, type: 0} +--- !u!23 &8149674613691108646 +MeshRenderer: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 135949056663158942} + m_Enabled: 1 + m_CastShadows: 1 + m_ReceiveShadows: 1 + m_DynamicOccludee: 1 + m_StaticShadowCaster: 0 + m_MotionVectors: 1 + m_LightProbeUsage: 1 + m_ReflectionProbeUsage: 1 + m_RayTracingMode: 2 + m_RayTraceProcedural: 0 + m_RenderingLayerMask: 1 + m_RendererPriority: 0 + m_Materials: + - {fileID: 10303, guid: 0000000000000000f000000000000000, type: 0} + m_StaticBatchInfo: + firstSubMesh: 0 + subMeshCount: 0 + m_StaticBatchRoot: {fileID: 0} + m_ProbeAnchor: {fileID: 0} + m_LightProbeVolumeOverride: {fileID: 0} + m_ScaleInLightmap: 1 + m_ReceiveGI: 1 + m_PreserveUVs: 0 + m_IgnoreNormalsForChartDetection: 0 + m_ImportantGI: 0 + m_StitchLightmapSeams: 1 + m_SelectedEditorRenderState: 3 + m_MinimumChartSize: 4 + m_AutoUVMaxDistance: 0.5 + m_AutoUVMaxAngle: 89 + m_LightmapParameters: {fileID: 0} + m_SortingLayerID: 0 + m_SortingLayer: 0 + m_SortingOrder: 0 + m_AdditionalVertexStreams: {fileID: 0} +--- !u!65 &7453252590388621785 +BoxCollider: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 135949056663158942} + m_Material: {fileID: 0} + m_IncludeLayers: + serializedVersion: 2 + m_Bits: 0 + m_ExcludeLayers: + serializedVersion: 2 + m_Bits: 0 + m_LayerOverridePriority: 0 + m_IsTrigger: 0 + m_ProvidesContacts: 0 + m_Enabled: 0 + serializedVersion: 3 + m_Size: {x: 1, y: 1, z: 1} + m_Center: {x: -3.4930155e-36, y: 0, z: 0} +--- !u!1 &3154420579555598162 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 213193100111257669} + - component: {fileID: 894333789048616995} + m_Layer: 0 + m_Name: Caster wheel + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &213193100111257669 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3154420579555598162} + serializedVersion: 2 + m_LocalRotation: {x: -0, y: -0, z: 7e-45, w: 1} + m_LocalPosition: {x: 0, y: 0, z: -0.1} + m_LocalScale: {x: 1, y: 1, z: 1} + m_ConstrainProportionsScale: 0 + m_Children: [] + m_Father: {fileID: 6798369561388671537} + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!135 &894333789048616995 +SphereCollider: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3154420579555598162} + m_Material: {fileID: 13400000, guid: 6311a2373d42743449d6eb1130ee618a, type: 2} + m_IncludeLayers: + serializedVersion: 2 + m_Bits: 0 + m_ExcludeLayers: + serializedVersion: 2 + m_Bits: 0 + m_LayerOverridePriority: 0 + m_IsTrigger: 0 + m_ProvidesContacts: 0 + m_Enabled: 1 + serializedVersion: 3 + m_Radius: 0.00001 + m_Center: {x: 0, y: 0, z: 0} +--- !u!1 &3377575892836316963 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 7997617267105515189} + - component: {fileID: 6773972788910618332} + - component: {fileID: 3720747953092717687} + m_Layer: 0 + m_Name: Left wheel + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &7997617267105515189 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3377575892836316963} + serializedVersion: 2 + m_LocalRotation: {x: -0, y: -0, z: -0, w: 1} + m_LocalPosition: {x: -0.08, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_ConstrainProportionsScale: 0 + m_Children: + - {fileID: 9144497505704104684} + m_Father: {fileID: 6798369561388671537} + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &6773972788910618332 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3377575892836316963} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: ddd5065d5e866894cbcb569c3a898ccb, type: 3} + m_Name: + m_EditorClassIdentifier: + participant: {fileID: 0} + maxSpeed: 5 + rotationSpeed: 0 +--- !u!135 &3720747953092717687 +SphereCollider: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 3377575892836316963} + m_Material: {fileID: 13400000, guid: 6311a2373d42743449d6eb1130ee618a, type: 2} + m_IncludeLayers: + serializedVersion: 2 + m_Bits: 0 + m_ExcludeLayers: + serializedVersion: 2 + m_Bits: 0 + m_LayerOverridePriority: 0 + m_IsTrigger: 0 + m_ProvidesContacts: 0 + m_Enabled: 1 + serializedVersion: 3 + m_Radius: 0.00001 + m_Center: {x: 0, y: 0, z: 0} +--- !u!1 &4326386140118987503 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 6798369561388671537} + - component: {fileID: 2975045340952151157} + - component: {fileID: 4118868690347991998} + m_Layer: 0 + m_Name: DifferentialDrive_disabled + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &6798369561388671537 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 4326386140118987503} + serializedVersion: 2 + m_LocalRotation: {x: 1.276433e-21, y: -1.082074e-19, z: -2.3980766e-36, w: 1} + m_LocalPosition: {x: 2.1932227e-37, y: -0.06941543, z: -1.3084181e-19} + m_LocalScale: {x: 1, y: 1, z: 1} + m_ConstrainProportionsScale: 0 + m_Children: + - {fileID: 7997617267105515189} + - {fileID: 2660791421672615984} + - {fileID: 693610334404048217} + - {fileID: 213193100111257669} + m_Father: {fileID: 0} + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &2975045340952151157 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 4326386140118987503} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 5c80bc2fd1d8aa14facb1ad4b6ccf7b3, type: 3} + m_Name: + m_EditorClassIdentifier: + participant: {fileID: 0} + leftWheel: {fileID: 6773972788910618332} + rightWheel: {fileID: 7425139233737877139} + casterWheel: {fileID: 894333789048616995} +--- !u!54 &4118868690347991998 +Rigidbody: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 4326386140118987503} + serializedVersion: 4 + m_Mass: 0.1 + m_Drag: 0 + m_AngularDrag: 0.05 + m_CenterOfMass: {x: 0, y: 0, z: 0} + m_InertiaTensor: {x: 1, y: 1, z: 1} + m_InertiaRotation: {x: 0, y: 0, z: 0, w: 1} + m_IncludeLayers: + serializedVersion: 2 + m_Bits: 0 + m_ExcludeLayers: + serializedVersion: 2 + m_Bits: 0 + m_ImplicitCom: 1 + m_ImplicitTensor: 1 + m_UseGravity: 1 + m_IsKinematic: 0 + m_Interpolate: 0 + m_Constraints: 0 + m_CollisionDetection: 0 +--- !u!1 &4927629757198460283 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 9144497505704104684} + - component: {fileID: 8353236605004633556} + - component: {fileID: 1652944855748388758} + m_Layer: 0 + m_Name: Cube + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &9144497505704104684 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 4927629757198460283} + serializedVersion: 2 + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 0.01, y: 0.05, z: 0.05} + m_ConstrainProportionsScale: 0 + m_Children: [] + m_Father: {fileID: 7997617267105515189} + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!33 &8353236605004633556 +MeshFilter: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 4927629757198460283} + m_Mesh: {fileID: 10202, guid: 0000000000000000e000000000000000, type: 0} +--- !u!23 &1652944855748388758 +MeshRenderer: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 4927629757198460283} + m_Enabled: 1 + m_CastShadows: 1 + m_ReceiveShadows: 1 + m_DynamicOccludee: 1 + m_StaticShadowCaster: 0 + m_MotionVectors: 1 + m_LightProbeUsage: 1 + m_ReflectionProbeUsage: 1 + m_RayTracingMode: 2 + m_RayTraceProcedural: 0 + m_RenderingLayerMask: 1 + m_RendererPriority: 0 + m_Materials: + - {fileID: 2100000, guid: c83c86eecb501964d9e4fd4074683016, type: 2} + m_StaticBatchInfo: + firstSubMesh: 0 + subMeshCount: 0 + m_StaticBatchRoot: {fileID: 0} + m_ProbeAnchor: {fileID: 0} + m_LightProbeVolumeOverride: {fileID: 0} + m_ScaleInLightmap: 1 + m_ReceiveGI: 1 + m_PreserveUVs: 0 + m_IgnoreNormalsForChartDetection: 0 + m_ImportantGI: 0 + m_StitchLightmapSeams: 1 + m_SelectedEditorRenderState: 3 + m_MinimumChartSize: 4 + m_AutoUVMaxDistance: 0.5 + m_AutoUVMaxAngle: 89 + m_LightmapParameters: {fileID: 0} + m_SortingLayerID: 0 + m_SortingLayer: 0 + m_SortingOrder: 0 + m_AdditionalVertexStreams: {fileID: 0} +--- !u!1 &5122915782100933114 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 2660791421672615984} + - component: {fileID: 7425139233737877139} + - component: {fileID: 8150853676422519226} + m_Layer: 0 + m_Name: Right wheel + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &2660791421672615984 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5122915782100933114} + serializedVersion: 2 + m_LocalRotation: {x: -0, y: -0, z: -0, w: 1} + m_LocalPosition: {x: 0.08, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_ConstrainProportionsScale: 0 + m_Children: + - {fileID: 2324127387642107303} + m_Father: {fileID: 6798369561388671537} + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!114 &7425139233737877139 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5122915782100933114} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: ddd5065d5e866894cbcb569c3a898ccb, type: 3} + m_Name: + m_EditorClassIdentifier: + participant: {fileID: 0} + maxSpeed: 5 + rotationSpeed: 0 +--- !u!135 &8150853676422519226 +SphereCollider: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5122915782100933114} + m_Material: {fileID: 13400000, guid: 6311a2373d42743449d6eb1130ee618a, type: 2} + m_IncludeLayers: + serializedVersion: 2 + m_Bits: 0 + m_ExcludeLayers: + serializedVersion: 2 + m_Bits: 0 + m_LayerOverridePriority: 0 + m_IsTrigger: 0 + m_ProvidesContacts: 0 + m_Enabled: 1 + serializedVersion: 3 + m_Radius: 0.00001 + m_Center: {x: 0, y: 0, z: 0} +--- !u!1 &5663058822615269368 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 2324127387642107303} + - component: {fileID: 7927909525546489266} + - component: {fileID: 1038362976985791207} + m_Layer: 0 + m_Name: Cube + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &2324127387642107303 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5663058822615269368} + serializedVersion: 2 + m_LocalRotation: {x: -0, y: -0, z: 7e-45, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 0.01, y: 0.049999997, z: 0.049999997} + m_ConstrainProportionsScale: 0 + m_Children: [] + m_Father: {fileID: 2660791421672615984} + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!33 &7927909525546489266 +MeshFilter: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5663058822615269368} + m_Mesh: {fileID: 10202, guid: 0000000000000000e000000000000000, type: 0} +--- !u!23 &1038362976985791207 +MeshRenderer: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 5663058822615269368} + m_Enabled: 1 + m_CastShadows: 1 + m_ReceiveShadows: 1 + m_DynamicOccludee: 1 + m_StaticShadowCaster: 0 + m_MotionVectors: 1 + m_LightProbeUsage: 1 + m_ReflectionProbeUsage: 1 + m_RayTracingMode: 2 + m_RayTraceProcedural: 0 + m_RenderingLayerMask: 1 + m_RendererPriority: 0 + m_Materials: + - {fileID: 2100000, guid: c83c86eecb501964d9e4fd4074683016, type: 2} + m_StaticBatchInfo: + firstSubMesh: 0 + subMeshCount: 0 + m_StaticBatchRoot: {fileID: 0} + m_ProbeAnchor: {fileID: 0} + m_LightProbeVolumeOverride: {fileID: 0} + m_ScaleInLightmap: 1 + m_ReceiveGI: 1 + m_PreserveUVs: 0 + m_IgnoreNormalsForChartDetection: 0 + m_ImportantGI: 0 + m_StitchLightmapSeams: 1 + m_SelectedEditorRenderState: 3 + m_MinimumChartSize: 4 + m_AutoUVMaxDistance: 0.5 + m_AutoUVMaxAngle: 89 + m_LightmapParameters: {fileID: 0} + m_SortingLayerID: 0 + m_SortingLayer: 0 + m_SortingOrder: 0 + m_AdditionalVertexStreams: {fileID: 0} diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs index 3226edc..24205c7 100644 --- a/Unity/SiteServer.cs +++ b/Unity/SiteServer.cs @@ -41,6 +41,9 @@ namespace RoboidControl.Unity { GameObject remoteParticipant = new GameObject("RemoteParticipant"); Participant participant = remoteParticipant.AddComponent(); participant.coreParticipant = e.participant; + participant.coreParticipant.component = participant; + participant.ipAddress = e.participant.ipAddress; + participant.port = e.participant.port; break; case ThingMsg.id: HandleThingEvent(e); diff --git a/Unity/Thing.cs b/Unity/Thing.cs index 711b910..575b72a 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -46,18 +46,18 @@ namespace RoboidControl.Unity { protected void Init(RoboidControl.Thing core) { this.core = core; this.core.component = this; - Participant[] participants = FindObjectsByType(FindObjectsSortMode.None); - foreach (Participant participant in participants) { - if (participant.ipAddress == core.owner.ipAddress && participant.port == core.owner.port) - this.owner = participant; - } - //this.owner = FindAnyObjectByType(); - //core.owner = this.owner.coreParticipant; + // This is wrong, it should get the owner, which is not the siteserver + // this.owner = FindAnyObjectByType(); + // core.owner = this.owner.coreParticipant; + this.owner = core.owner.component; if (core.parent != null && core.parent.component != null) { this.transform.SetParent(core.parent.component.transform, false); this.transform.localPosition = Vector3.zero; } + else { + this.transform.SetParent(core.owner.component.transform, false); + } if (core.position != null) this.transform.localPosition = core.position.ToVector3(); @@ -88,6 +88,7 @@ namespace RoboidControl.Unity { /// protected virtual void FixedUpdate() { UpdateThing(); + core.orientation = LinearAlgebra.SwingTwist.FromQuaternion(this.transform.localRotation); } /// @@ -95,8 +96,8 @@ 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; } @@ -113,7 +114,7 @@ namespace RoboidControl.Unity { case ThingMsg.id: Debug.Log($"{this.core.id} Handle Thing"); if (core.parent == null) - this.transform.SetParent(null, true); + this.transform.SetParent(core.owner.component.transform, true); else if (core.parent.component != null) this.transform.SetParent(core.parent.component.transform, true); break; @@ -134,7 +135,7 @@ namespace RoboidControl.Unity { this.HandlePose(); break; case BinaryMsg.Id: - // Debug.Log($"{this.core.id} Handle Binary"); + Debug.Log($"{this.core.id} Handle Binary"); this.HandleBinary(); break; } @@ -195,19 +196,19 @@ namespace RoboidControl.Unity { SkinnedMeshRenderer[] meshRenderers = parentTransform.GetComponentsInChildren(); #if pHUMANOID4 - if (parentThing.objectType == 7) { - HumanoidControl hc = parentThing.gameObject.GetComponent(); - if (hc == null) - hc = parentThing.gameObject.AddComponent(); + // if (parentThing.objectType == 7) { + // HumanoidControl hc = parentThing.gameObject.GetComponent(); + // if (hc == null) + // hc = parentThing.gameObject.AddComponent(); - foreach (SkinnedMeshRenderer meshRenderer in meshRenderers) { - if (meshRenderer.rootBone != null) { - Debug.Log("Found a skinned mesh with bones"); - hc.RetrieveBonesFrom(meshRenderer.rootBone); - break; - } - } - } + // foreach (SkinnedMeshRenderer meshRenderer in meshRenderers) { + // if (meshRenderer.rootBone != null) { + // Debug.Log("Found a skinned mesh with bones"); + // hc.RetrieveBonesFrom(meshRenderer.rootBone); + // break; + // } + // } + // } #endif parentTransform.localScale = Vector3.one; if (meshRenderers.Length > 0) { @@ -259,6 +260,7 @@ namespace RoboidControl.Unity { /// This can update the position and/or orientation when the velocity of the thing is zero. /// If a velocity is not zero, the position and/or orientation update will be ignored protected virtual void HandlePose() { + this.transform.localRotation = core.orientation.ToQuaternion(); if (core.linearVelocity.distance == 0) this.transform.localPosition = core.position.ToVector3(); if (core.angularVelocity.distance == 0) diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs index 3e17c46..9702ffb 100644 --- a/Unity/TouchSensor.cs +++ b/Unity/TouchSensor.cs @@ -45,8 +45,6 @@ namespace RoboidControl.Unity { return touchSensor; } - public bool touchedSomething = false; - /// /// Handle touch trigger collider enter event /// @@ -54,40 +52,22 @@ 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)); } - - // private void OnTriggerStay(Collider other) { - // // Don't detect trigger colliders - // if (other.isTrigger) - // return; - // // Don't touch yourself - // if (this.transform.root == other.transform.root) - // return; - - // Debug.Log("Touching..."); - // this.touchedSomething = true; - // this.coreSensor.touchedSomething = true; - // } - - /// - /// Handl touch trigger collider exit event - /// - /// The collider which exited our trigger collider - private void OnTriggerExit(Collider other) { + /// + /// Handl touch trigger collider exit event + /// + /// The collider which exited our trigger collider + private void OnTriggerExit(Collider other) { 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 0e52270..41d427f 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 : Thing { + public class Wheel : Motor { /// /// Create the Unity representation /// /// The core motor /// The Unity representation of a motorised wheel - public static new Wheel Create(RoboidControl.Thing core) { + public static Wheel Create(RoboidControl.Motor core, float wheelRadius) { GameObject prefab = (GameObject)Resources.Load("Wheel"); if (prefab != null) { // Use resource prefab when available @@ -38,7 +38,7 @@ namespace RoboidControl.Unity { GameObject gameObj = Instantiate(prefab); Wheel component = gameObj.GetComponent(); if (component != null) - component.core = new RoboidControl.Thing(RoboidControl.Thing.Type.UncontrolledMotor, false); + component.core = new RoboidControl.Thing(RoboidControl.Thing.Type.UncontrolledMotor); return component; } else { @@ -49,7 +49,8 @@ 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 = new(participant.coreParticipant, RoboidControl.Thing.Type.UncontrolledMotor, thingId, false); + core = RoboidControl.Thing.CreateRemote(participant.coreParticipant, RoboidControl.Thing.Type.UncontrolledMotor, thingId); else { ; } @@ -62,8 +63,6 @@ namespace RoboidControl.Unity { } } - public float wheelRadius = 0; - private static PhysicMaterial _slidingWheel; public static PhysicMaterial slidingWheel { get { diff --git a/src/Participant.cs b/src/Participant.cs index 572ec93..066cb44 100644 --- a/src/Participant.cs +++ b/src/Participant.cs @@ -14,6 +14,13 @@ namespace RoboidControl { /// It also maintains the communcation information to contact the participant. /// It is used as a basis for the local participant, but also as a reference to remote participants. public class Participant { + + private Participant() { + this.root = Thing.CreateRoot(this); + this.root.name = "Root"; + this.Add(this.root); + } + /// /// Create a new participant with the given communcation info /// @@ -22,10 +29,20 @@ namespace RoboidControl { public Participant(string ipAddress, int port, Participant localParticipant = null) { this.ipAddress = ipAddress; this.port = port; + + this.root = Thing.CreateRoot(this); + this.root.name = "Root"; + if (localParticipant != null) this.udpClient = localParticipant.udpClient; } + public static Participant localParticipant = new(); + public static void ReplaceLocalParticipant(Participant participant) { + Participant.localParticipant = participant; + } + + public Thing root = null; /// /// The Ip Address of a participant. When the participant is local, this contains 0.0.0.0 /// @@ -35,6 +52,14 @@ namespace RoboidControl { /// public int port = 0; +#if UNITY_5_3_OR_NEWER + /// + /// A reference to the representation of the thing in Unity + /// + [NonSerialized] + public Unity.Participant component = null; +#endif + public UdpClient udpClient = null; /// diff --git a/src/Participants/ParticipantUDP.cs b/src/Participants/ParticipantUDP.cs index a133948..941d117 100644 --- a/src/Participants/ParticipantUDP.cs +++ b/src/Participants/ParticipantUDP.cs @@ -35,6 +35,7 @@ namespace RoboidControl { if (this.port == 0) this.isIsolated = true; Participant.AddParticipant(this); + Participant.ReplaceLocalParticipant(this); } /// /// Create a participant which will try to connect to a site. @@ -79,6 +80,7 @@ namespace RoboidControl { this.remoteSite = new Participant(ipAddress, port, this); Participant.AddParticipant(this); + Participant.ReplaceLocalParticipant(this); } private static ParticipantUDP isolatedParticipant = null; diff --git a/src/Participants/SiteServer.cs b/src/Participants/SiteServer.cs index 53e21c1..519f425 100644 --- a/src/Participants/SiteServer.cs +++ b/src/Participants/SiteServer.cs @@ -132,10 +132,9 @@ 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), - Thing.Type.UncontrolledMotor => new Motor(sender, msg.thingId), - _ => new Thing(sender, msg.thingType, msg.thingId), + //Thing.Type.TouchSensor => new TouchSensor(sender, msg.thingId), + //Thing.Type.DifferentialDrive => new DifferentialDrive(sender, msg.thingId), + _ => Thing.CreateRemote(sender, msg.thingType, msg.thingId) }; } diff --git a/src/Thing.cs b/src/Thing.cs index af90a11..e642388 100644 --- a/src/Thing.cs +++ b/src/Thing.cs @@ -26,10 +26,11 @@ namespace RoboidControl { public const byte ControlledMotor = 0x06; public const byte UncontrolledMotor = 0x07; public const byte Servo = 0x08; - public const byte IncrementalEncoder = 0x19; + public const byte RelativeEncoder = 0x19; // Other + public const byte Root = 0x10; public const byte Roboid = 0x09; - public const byte HUmanoid = 0x0A; + public const byte Humanoid = 0x0A; public const byte ExternalSensor = 0x0B; public const byte Animator = 0x0C; public const byte DifferentialDrive = 0x0D; @@ -37,6 +38,29 @@ namespace RoboidControl { #region Init + private Thing(Participant owner) { + this.type = Type.Root; + + this.positionUpdated = true; + this.orientationUpdated = true; + this.hierarchyChanged = true; + + this.owner = owner; + this.parent = null; + } + + public static Thing CreateRoot(Participant owner) { + return new Thing(owner); + } + + static Thing localRoot { + get { + Participant participant = Participant.localParticipant; + return participant.root; + } + } + + /* /// /// Create a new thing without communication abilities /// @@ -66,6 +90,7 @@ namespace RoboidControl { this.owner.updateQueue.Enqueue(e); } } + */ /// /// Create a new child thing @@ -75,26 +100,39 @@ 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, byte thingId = 0, bool invokeEvent = true) : this(parent.owner, thingType, thingId, invokeEvent) { this.parent = parent; } + */ + public Thing(byte thingType = Type.Undetermined, Thing parent = default) { + this.type = thingType; - // /// - // /// Create a new thing for the given participant - // /// - // /// The participant owning the thing - // /// The network ID of the thing - // /// The ID of the thing - // /// The type of thing - // public Thing(Participant owner, byte networkId, byte thingId, byte thingType = 0) { - // this.owner = owner; - // this.id = thingId; - // this.type = thingType; - // this.networkId = networkId; - // // Console.Write($"New thing added to {owner}"); - // this.owner.Add(this); - // InvokeNewThing(this); - // } + this.positionUpdated = true; + this.orientationUpdated = true; + this.hierarchyChanged = true; + + if (parent == default) + this.parent = Participant.localParticipant.root; + else + this.parent = parent; + + this.owner = this.parent.owner; + this.owner.Add(this, true); + + Participant.UpdateEvent e = new() { + messageId = ThingMsg.id, + thing = this + }; + this.owner.updateQueue.Enqueue(e); + } + + public static Thing CreateRemote(Participant owner, byte thingType, byte thingId) { + Thing remoteThing = new(thingType, owner.root) { + id = thingId + }; + return remoteThing; + } /// /// Function which can be used to create components in external engines. @@ -302,6 +340,7 @@ namespace RoboidControl { if (_orientation != value) { _orientation = value; orientationUpdated = true; + updateQueue.Enqueue(new CoreEvent(PoseMsg.Id)); //OnOrientationChanged?.Invoke(); } } @@ -383,7 +422,7 @@ namespace RoboidControl { this.orientationUpdated = false; this.linearVelocityUpdated = false; this.angularVelocityUpdated = false; - //this.hierarchyChanged = false; + this.hierarchyChanged = false; // should recurse over children... if (recurse) { diff --git a/src/Things/ControlledMotor.cs b/src/Things/ControlledMotor.cs new file mode 100644 index 0000000..fb5ad79 --- /dev/null +++ b/src/Things/ControlledMotor.cs @@ -0,0 +1,84 @@ +namespace RoboidControl { + + /// @brief A motor with speed control + /// It uses a feedback loop from an encoder to regulate the speed + /// The speed is measured in revolutions per second. + public class ControlledMotor : Motor { + public ControlledMotor(Thing parent, RelativeEncoder encoder) : base(parent) { + this.encoder = encoder; + } + // Upgrade an existing motor with an encoder + public ControlledMotor(Motor motor, RelativeEncoder encoder) : base(motor.parent) { + this.motor = motor; + this.encoder = encoder; + } + + readonly RelativeEncoder encoder; + readonly Motor motor; + + public override float targetSpeed { + get { + if (this.motor != null) + return this.motor.targetSpeed; + else + return base.targetSpeed; + } + set { + if (this.motor != null) + this.motor.targetSpeed = value; + else + base.targetSpeed = value; + } + } + + private float _targetAngularSpeed; // In degrees per second + public virtual float targetAngularSpeed { + get => _targetAngularSpeed; + set { + if (value != this._targetAngularSpeed) { + this._targetAngularSpeed = value; + updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id)); + owner.Send(new BinaryMsg(this)); + } + } + } + /// @brief Get the actual speed from the encoder + /// @return The speed in angle per second + public virtual float actualAngularSpee { + get => encoder.angularSpeed; + } + + float pidP = 0.1F; + float pidD = 0.0F; + //float pidI = 0.0F; + + public override void Update(ulong currentTimeMs, bool recurse = false) { + float actualSpeed = this.encoder.angularSpeed; + // Simplified rotation direction, shouldbe improved + // This goes wrong when the target speed is inverted and the motor axcle is still turning in the old direction + if (this.targetAngularSpeed < 0) + actualSpeed = -actualSpeed; + + float deltaTime = (currentTimeMs - this.lastUpdateTime) / 1000.0f; + + float error = this.targetAngularSpeed - actualSpeed; + float errorChange = (error - lastError) * deltaTime; + + float delta = error * pidP + errorChange * pidD; + + // Update the motor speed + if (motor != null) + motor.targetSpeed += delta; + else + base.targetSpeed += delta; + this.lastUpdateTime = currentTimeMs; + } + + ulong lastUpdateTime = 0; + float lastError = 0; + + // enum Direction { Forward = 1, Reverse = -1 }; + // Direction rotationDirection; + }; + +} // namespace RoboidControl diff --git a/src/Things/DifferentialDrive.cs b/src/Things/DifferentialDrive.cs index c9b4d00..039d26c 100644 --- a/src/Things/DifferentialDrive.cs +++ b/src/Things/DifferentialDrive.cs @@ -1,3 +1,4 @@ +using System; using LinearAlgebra; namespace RoboidControl { @@ -6,11 +7,13 @@ 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(bool invokeEvent = true) : base(Type.DifferentialDrive, invokeEvent) { } + public DifferentialDrive() : base(Type.DifferentialDrive) { } + /// /// Create a differential drive for a participant /// @@ -28,15 +31,16 @@ namespace RoboidControl { // sendBinary = true; // owner.Send(new BinaryMsg(owner.networkId, this)); // this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id)); - } + */ + /// /// 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) { } + public DifferentialDrive(Thing parent) : base(Type.DifferentialDrive, parent) { } /// @brief Configures the dimensions of the drive /// @param wheelDiameter The diameter of the wheels in meters @@ -47,7 +51,7 @@ namespace RoboidControl { /// @sa SetLinearVelocity SetAngularVelocity public void SetDriveDimensions(float wheelDiameter, float wheelSeparation = 0) { this._wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2; - this.rpsToMs = wheelDiameter * Angle.pi; + this.rpsToMs = wheelDiameter * (float)Math.PI; if (wheelSeparation > 0) { wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation; @@ -101,9 +105,9 @@ namespace RoboidControl { /// Positive moves the robot in the forward direction. public void SetWheelAngularVelocity(float angularSpeedLeft, float angularSpeedRight) { // This only works when the motor is a motor with encoder - if (this.leftWheel is EncoderMotor leftMotor) + if (this.leftWheel is ControlledMotor leftMotor) leftMotor.targetAngularSpeed = angularSpeedLeft; - if (this.rightWheel is EncoderMotor rightMotor) + if (this.rightWheel is ControlledMotor rightMotor) rightMotor.targetAngularSpeed = angularSpeedRight; } @@ -160,8 +164,7 @@ namespace RoboidControl { public override void ProcessBinary(byte[] data) { byte ix = 0; byte leftWheelId = data[ix++]; - Thing leftMotor = this.owner.Get(leftWheelId); - this.leftWheel = leftMotor as Motor; + this.leftWheel = this.owner.Get(leftWheelId) as Motor; byte rightWheelId = data[ix++]; this.rightWheel = this.owner.Get(rightWheelId) as Motor; this._wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix); diff --git a/src/Things/DigitalSensor.cs b/src/Things/DigitalSensor.cs index 2993580..3d1a19c 100644 --- a/src/Things/DigitalSensor.cs +++ b/src/Things/DigitalSensor.cs @@ -6,6 +6,7 @@ namespace RoboidControl { /// A sensor which can detect touches /// public class DigitalSensor : Thing { + /* /// /// Create a digital sensor without communication abilities /// @@ -18,13 +19,14 @@ 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 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) { } + public DigitalSensor(Thing parent) : base(Type.Switch, parent) { } /// /// Value which is true when the sensor is touching something, false otherwise diff --git a/src/Things/DistanceSensor.cs b/src/Things/DistanceSensor.cs index ad5f785..def58f9 100644 --- a/src/Things/DistanceSensor.cs +++ b/src/Things/DistanceSensor.cs @@ -9,19 +9,23 @@ namespace RoboidControl { /// public float distance = 0; + /* /// /// Constructor for a new distance sensor /// /// The participant for which the sensor is needed public DistanceSensor(Participant participant) : base(participant, Type.Undetermined) { } + /// /// Create a distance sensor with the given ID /// /// The participant for with the sensor is needed /// The network ID of the sensor /// The ID of the thing - public DistanceSensor(Participant owner, byte thingId) : base(owner, Type.TemperatureSensor, thingId) { - } + public DistanceSensor(Participant owner, byte thingId) : base(owner, Type.TemperatureSensor, thingId) {} + */ + public DistanceSensor(Thing parent): base(Type.DistanceSensor, parent) {} + #if UNITY_5_3_OR_NEWER /// @copydoc Passer::RoboidControl::Thing::CreateComponent diff --git a/src/Things/Motor.cs b/src/Things/Motor.cs index 4534400..e4ecad8 100644 --- a/src/Things/Motor.cs +++ b/src/Things/Motor.cs @@ -3,10 +3,8 @@ 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) { } - public Motor(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.UncontrolledMotor, thingId, invokeEvent) { } - + //public Motor(bool invokeEvent = true) : base(Type.UncontrolledMotor, invokeEvent) { } + public Motor(Thing parent) : base(Type.UncontrolledMotor, parent) { } /// @brief Motor turning direction public enum Direction { @@ -40,7 +38,7 @@ namespace RoboidControl { return data; } public override void ProcessBinary(byte[] data) { - this.targetSpeed = (float)(sbyte)data[0] / 127; + this.targetSpeed = (float)data[0] / 127; } } diff --git a/src/Things/RelativeEncoder.cs b/src/Things/RelativeEncoder.cs index 5183f91..aee9d7c 100644 --- a/src/Things/RelativeEncoder.cs +++ b/src/Things/RelativeEncoder.cs @@ -9,7 +9,12 @@ namespace RoboidControl { /// full rotation /// @param distancePerRevolution The distance a wheel travels per full /// rotation + /* public RelativeEncoder(bool invokeEvent = true) : base(Type.IncrementalEncoder, invokeEvent) { } + */ + public RelativeEncoder(Thing parent = default) : base(Type.RelativeEncoder, parent) { + + } protected float _rotationSpeed = 0; /// @brief Get the rotation speed since the previous call diff --git a/src/Things/TemperatureSensor.cs b/src/Things/TemperatureSensor.cs index 6475d06..ecf548e 100644 --- a/src/Things/TemperatureSensor.cs +++ b/src/Things/TemperatureSensor.cs @@ -6,6 +6,7 @@ namespace RoboidControl { /// A temperature sensor /// public class TemperatureSensor : Thing { + /* /// /// Create a temperature sensor without communication abilities /// @@ -18,14 +19,14 @@ namespace RoboidControl { /// 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) { } + public TemperatureSensor(Thing parent) : base(Type.TemperatureSensor, parent) { } /// /// The measured temperature diff --git a/src/Things/TouchSensor.cs b/src/Things/TouchSensor.cs index 4443eae..e2db699 100644 --- a/src/Things/TouchSensor.cs +++ b/src/Things/TouchSensor.cs @@ -6,6 +6,7 @@ namespace RoboidControl { /// A sensor which can detect touches /// public class TouchSensor : Thing { + /* /// /// Create a touch sensor without communication abilities /// @@ -18,13 +19,14 @@ 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 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) { + public TouchSensor(Thing parent) : base(Type.TouchSensor, parent) { this.name = "TouchSensor"; } @@ -35,10 +37,9 @@ namespace RoboidControl { public bool touchedSomething { get { return _touchedSomething; } set { - //if (_touchedSomething != value) { + if (_touchedSomething != value) { _touchedSomething = value; - owner.Send(new BinaryMsg(this)); - //} + } touchUpdated = true; } } @@ -58,24 +59,21 @@ 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 (this.component == null || !touchUpdated) + if (!touchUpdated) return Array.Empty(); - Unity.TouchSensor touchComponent = this.component as Unity.TouchSensor; byte[] bytes = new byte[1]; - bytes[0] = (byte)(touchComponent.touchedSomething ? 1 : 0); + bytes[0] = (byte)(touchedSomething ? 1 : 0); touchUpdated = false; - Console.WriteLine($"Sending touch {bytes[0]}"); return bytes; } - 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.externalTouch = (bytes[0] == 1); + this.touchedSomething |= (bytes[0] == 1); } } } \ No newline at end of file diff --git a/test/UnitTest1.cs b/test/UnitTest1.cs index 46a3864..9fc8f79 100644 --- a/test/UnitTest1.cs +++ b/test/UnitTest1.cs @@ -65,7 +65,7 @@ namespace RoboidControl.test { public void Test_ThingMsg() { SiteServer siteServer = new(7681); ParticipantUDP participant = new("127.0.0.1", 7681, 7682); - Thing thing = new(participant) { + Thing thing = new() { name = "First Thing", modelUrl = "https://passer.life/extras/ant.jpg" };