RoboidControl/RoboidControl-csharp
RoboidControl/RoboidControl-csharp
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases Activity

Merge commit '2cdf8f61037fe583c4fa2e9a856e899c255cf638'

Browse Source
This commit is contained in:
Pascal Serrarens 2025-06-02 08:55:40 +02:00
commit 9305cad6d6
34 changed files with 1316 additions and 305 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Examples/BB2B/BB2B.cs
View File

@ -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);

6
Examples/BB2B/BB2B_Encoder.cs
View File

@ -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);

2
Examples/BB2B/Program.cs
View File

@ -3,7 +3,7 @@ using RoboidControl;
class Program {
static void Main() {
BB2B bb2b = new(ParticipantUDP.Isolated());
BB2B bb2b = new();
while (true) {
bb2b.Update();

177
LinearAlgebra/src/Angle.cs
View File

@ -1,17 +1,128 @@
using System;
using System.Reflection.Emit;
namespace LinearAlgebra {
/// <summary>
/// %Angle utilities
/// </summary>
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; }
}
/// <summary>
/// Get the sign of the angle
/// </summary>
/// <param name="a">The angle</param>
/// <returns>-1 when the angle is negative, 1 when it is positive and 0 in all other cases</returns>
public static int Sign(Angle a) {
if (a.value < 0)
return -1;
if (a.value > 0)
return 1;
return 0;
}
/// <summary>
/// Returns the magnitude of the angle
/// </summary>
/// <param name="a">The angle</param>
/// <returns>The positive magnitude of the angle</returns>
/// Negative values are negated to get a positive result
public static Angle Abs(Angle a) {
if (Sign(a) < 0)
return -a;
else
return a;
}
/// <summary>
/// Negate the angle
/// </summary>
/// <param name="a">The angle</param>
/// <returns>The negated angle</returns>
/// 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;
}
/// <summary>
/// Subtract two angles
/// </summary>
/// <param name="a1">Angle 1</param>
/// <param name="a2">Angle 2</param>
/// <returns>The result of the subtraction</returns>
public static Angle operator -(Angle a1, Angle a2) {
Angle r = new(a1.value - a2.value);
return r;
}
/// <summary>
/// Add two angles
/// </summary>
/// <param name="a1">Angle 1</param>
/// <param name="a2">Angle 2</param>
/// <returns>The result of the addition</returns>
public static Angle operator +(Angle a1, Angle a2) {
Angle r = new(a1.value + a2.value);
return r;
}
/// <summary>
/// Clamp the angle between the given min and max values
@ -21,11 +132,38 @@ namespace LinearAlgebra {
/// <param name="max">The maximum angle</param>
/// <returns>The clamped angle</returns>
/// 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);
}
/// <summary>
/// Rotate from one angle to the other with a maximum degrees
/// </summary>
/// <param name="fromAngle">Starting angle</param>
/// <param name="toAngle">Target angle</param>
/// <param name="maxAngle">Maximum angle to rotate</param>
/// <returns>The resulting angle</returns>
/// 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;
}
}
/// <summary>
/// %Angle utilities
/// </summary>
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;
/// <summary>
/// Determine the angle difference, result is a normalized angle
/// </summary>
@ -53,21 +191,6 @@ namespace LinearAlgebra {
return angle;
}
/// <summary>
/// Rotate from one angle to the other with a maximum degrees
/// </summary>
/// <param name="fromAngle">Starting angle</param>
/// <param name="toAngle">Target angle</param>
/// <param name="maxAngle">Maximum angle to rotate</param>
/// <returns>The resulting angle</returns>
/// 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;
}
/// <summary>
/// Map interval of angles between vectors [0..Pi] to interval [0..1]
/// </summary>

6
LinearAlgebra/src/Direction.cs
View File

@ -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);

4
LinearAlgebra/src/Spherical.cs
View File

@ -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);

9
LinearAlgebra/src/SwingTwist.cs
View File

@ -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
}

236
LinearAlgebra/test/AngleTest.cs
View File

@ -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");
}
}

19
LinearAlgebra/test/DirectionTest.cs Normal file
View File

@ -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

27
LinearAlgebra/test/SphericalTest.cs
View File

@ -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)");
}
}
}

18
README.md
View File

@ -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.

89
Unity/DifferentialDrive.cs
View File

@ -42,8 +42,6 @@ namespace RoboidControl.Unity {
return differentialDrive;
}
public Motor leftMotor;
public Motor rightMotor;
/// <summary>
/// The left wheel of the differential drive
/// </summary>
@ -73,8 +71,41 @@ namespace RoboidControl.Unity {
/// Handle the binary event indicating a configuration change
/// </summary>
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<Thing>();
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<SphereCollider>();
leftWheelCollider.radius = coreDrive.wheelRadius;
}
// Destroy any (generic) thing with the same id
if (rightWheel == null) {
Thing[] things = FindObjectsOfType<Thing>();
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<SphereCollider>();
rightWheelCollider.radius = coreDrive.wheelRadius;
}
if (casterWheel == null) {
GameObject gameObj = new("Caster wheel");
@ -82,30 +113,11 @@ namespace RoboidControl.Unity {
casterWheel = gameObj.AddComponent<SphereCollider>();
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);
}
/// <summary>
@ -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;
}
}
}

88
Unity/Editor/Configurator.cs Normal file
View File

@ -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<string> 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<string>();
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);
}
}
}
}

6
Unity/Motor.cs
View File

@ -34,8 +34,8 @@ namespace RoboidControl.Unity {
motor = gameObj.AddComponent<Motor>();
motor.Init(coreMotor);
// Rigidbody rb = gameObj.AddComponent<Rigidbody>();
// rb.isKinematic = true;
Rigidbody rb = gameObj.AddComponent<Rigidbody>();
rb.isKinematic = true;
}
return motor;
}
@ -43,7 +43,7 @@ namespace RoboidControl.Unity {
/// <summary>
/// The maximum rotation speed of the motor in rotations per second
/// </summary>
public float maxSpeed = 2;
public float maxSpeed = 5;
/// <summary>
/// The actual rotation speed in rotations per second

22
Unity/Participant.cs
View File

@ -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<Thing>();
// 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) {

2
Unity/RelativeEncoder.cs
View File

@ -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;
}

546
Unity/Resources/DifferentialDrive_disabled.prefab Normal file
View File

@ -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}

3
Unity/SiteServer.cs
View File

@ -41,6 +41,9 @@ namespace RoboidControl.Unity {
GameObject remoteParticipant = new GameObject("RemoteParticipant");
Participant participant = remoteParticipant.AddComponent<Participant>();
participant.coreParticipant = e.participant;
participant.coreParticipant.component = participant;
participant.ipAddress = e.participant.ipAddress;
participant.port = e.participant.port;
break;
case ThingMsg.id:
HandleThingEvent(e);

48
Unity/Thing.cs
View File

@ -46,18 +46,18 @@ namespace RoboidControl.Unity {
protected void Init(RoboidControl.Thing core) {
this.core = core;
this.core.component = this;
Participant[] participants = FindObjectsByType<Participant>(FindObjectsSortMode.None);
foreach (Participant participant in participants) {
if (participant.ipAddress == core.owner.ipAddress && participant.port == core.owner.port)
this.owner = participant;
}
//this.owner = FindAnyObjectByType<SiteServer>();
//core.owner = this.owner.coreParticipant;
// This is wrong, it should get the owner, which is not the siteserver
// this.owner = FindAnyObjectByType<SiteServer>();
// 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 {
/// </summary>
protected virtual void FixedUpdate() {
UpdateThing();
core.orientation = LinearAlgebra.SwingTwist.FromQuaternion(this.transform.localRotation);
}
/// <summary>
@ -95,8 +96,8 @@ namespace RoboidControl.Unity {
/// </summary>
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<SkinnedMeshRenderer>();
#if pHUMANOID4
if (parentThing.objectType == 7) {
HumanoidControl hc = parentThing.gameObject.GetComponent<HumanoidControl>();
if (hc == null)
hc = parentThing.gameObject.AddComponent<HumanoidControl>();
// if (parentThing.objectType == 7) {
// HumanoidControl hc = parentThing.gameObject.GetComponent<HumanoidControl>();
// if (hc == null)
// hc = parentThing.gameObject.AddComponent<HumanoidControl>();
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)

32
Unity/TouchSensor.cs
View File

@ -45,8 +45,6 @@ namespace RoboidControl.Unity {
return touchSensor;
}
public bool touchedSomething = false;
/// <summary>
/// Handle touch trigger collider enter event
/// </summary>
@ -59,35 +57,17 @@ namespace RoboidControl.Unity {
if (this.transform.root == other.transform.root)
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;
// }
/// <summary>
/// Handl touch trigger collider exit event
/// </summary>
/// <param name="other">The collider which exited our trigger collider </param>
private void OnTriggerExit(Collider other) {
/// <summary>
/// Handl touch trigger collider exit event
/// </summary>
/// <param name="other">The collider which exited our trigger collider </param>
private void OnTriggerExit(Collider other) {
if (other.isTrigger)
return;
Debug.Log("TOUCH END!");
this.touchedSomething = false;
this.coreSensor.touchedSomething = false;
}
}

11
Unity/Wheel.cs
View File

@ -6,13 +6,13 @@ namespace RoboidControl.Unity {
/// <summary>
/// The Unity representation of a Roboid Control wheel
/// </summary>
public class Wheel : Thing {
public class Wheel : Motor {
/// <summary>
/// Create the Unity representation
/// </summary>
/// <param name="core">The core motor</param>
/// <returns>The Unity representation of a motorised wheel</returns>
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<Wheel>();
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<SiteServer>();
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 {

25
src/Participant.cs
View File

@ -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);
}
/// <summary>
/// Create a new participant with the given communcation info
/// </summary>
@ -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;
/// <summary>
/// The Ip Address of a participant. When the participant is local, this contains 0.0.0.0
/// </summary>
@ -35,6 +52,14 @@ namespace RoboidControl {
/// </summary>
public int port = 0;
#if UNITY_5_3_OR_NEWER
/// <summary>
/// A reference to the representation of the thing in Unity
/// </summary>
[NonSerialized]
public Unity.Participant component = null;
#endif
public UdpClient udpClient = null;
/// <summary>

2
src/Participants/ParticipantUDP.cs
View File

@ -35,6 +35,7 @@ namespace RoboidControl {
if (this.port == 0)
this.isIsolated = true;
Participant.AddParticipant(this);
Participant.ReplaceLocalParticipant(this);
}
/// <summary>
/// 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;

7
src/Participants/SiteServer.cs
View File

@ -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)
};
}

77
src/Thing.cs
View File

@ -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;
}
}
/*
/// <summary>
/// Create a new thing without communication abilities
/// </summary>
@ -66,6 +90,7 @@ namespace RoboidControl {
this.owner.updateQueue.Enqueue(e);
}
}
*/
/// <summary>
/// Create a new child thing
@ -75,26 +100,39 @@ namespace RoboidControl {
/// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
/// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
/// <note>The owner will be the same as the owner of the parent thing</note>
/*
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;
// /// <summary>
// /// Create a new thing for the given participant
// /// </summary>
// /// <param name="owner">The participant owning the thing</param>
// /// <param name="networkId">The network ID of the thing</param>
// /// <param name="thingId">The ID of the thing</param>
// /// <param name="thingType">The type of thing</param>
// 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;
}
/// <summary>
/// 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) {

84
src/Things/ControlledMotor.cs Normal file
View File

@ -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

19
src/Things/DifferentialDrive.cs
View File

@ -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 {
/*
/// <summary>
/// Create a differential drive without communication abilities
/// </summary
/// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
public DifferentialDrive(bool invokeEvent = true) : base(Type.DifferentialDrive, invokeEvent) { }
public DifferentialDrive() : base(Type.DifferentialDrive) { }
/// <summary>
/// Create a differential drive for a participant
/// </summary>
@ -28,15 +31,16 @@ namespace RoboidControl {
// sendBinary = true;
// owner.Send(new BinaryMsg(owner.networkId, this));
// this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id));
}
*/
/// <summary>
/// Create a new child differential drive
/// </summary>
/// <param name="parent">The parent thing</param>
/// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
/// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
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);

4
src/Things/DigitalSensor.cs
View File

@ -6,6 +6,7 @@ namespace RoboidControl {
/// A sensor which can detect touches
/// </summary>
public class DigitalSensor : Thing {
/*
/// <summary>
/// Create a digital sensor without communication abilities
/// </summary>
@ -18,13 +19,14 @@ namespace RoboidControl {
/// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
/// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
public DigitalSensor(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.Switch, thingId, invokeEvent) { }
*/
/// <summary>
/// Create a new child digital sensor
/// </summary>
/// <param name="parent">The parent thing</param>
/// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
/// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
public DigitalSensor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.Switch, thingId, invokeEvent) { }
public DigitalSensor(Thing parent) : base(Type.Switch, parent) { }
/// <summary>
/// Value which is true when the sensor is touching something, false otherwise

8
src/Things/DistanceSensor.cs
View File

@ -9,19 +9,23 @@ namespace RoboidControl {
/// </summary>
public float distance = 0;
/*
/// <summary>
/// Constructor for a new distance sensor
/// </summary>
/// <param name="participant">The participant for which the sensor is needed</param>
public DistanceSensor(Participant participant) : base(participant, Type.Undetermined) { }
/// <summary>
/// Create a distance sensor with the given ID
/// </summary>
/// <param name="owner">The participant for with the sensor is needed</param>
/// <param name="networkId">The network ID of the sensor</param>
/// <param name="thingId">The ID of the thing</param>
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

8
src/Things/Motor.cs
View File

@ -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;
}
}

5
src/Things/RelativeEncoder.cs
View File

@ -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

5
src/Things/TemperatureSensor.cs
View File

@ -6,6 +6,7 @@ namespace RoboidControl {
/// A temperature sensor
/// </summary>
public class TemperatureSensor : Thing {
/*
/// <summary>
/// Create a temperature sensor without communication abilities
/// </summary>
@ -18,14 +19,14 @@ namespace RoboidControl {
/// <param name="thingId">The ID of the thing</param>
/// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
public TemperatureSensor(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.TemperatureSensor, thingId, invokeEvent) { }
*/
/// <summary>
/// Create a new child temperature sensor
/// </summary>
/// <param name="parent">The parent thing</param>
/// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
/// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
public TemperatureSensor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.TemperatureSensor, thingId, invokeEvent) { }
public TemperatureSensor(Thing parent) : base(Type.TemperatureSensor, parent) { }
/// <summary>
/// The measured temperature

18
src/Things/TouchSensor.cs
View File

@ -6,6 +6,7 @@ namespace RoboidControl {
/// A sensor which can detect touches
/// </summary>
public class TouchSensor : Thing {
/*
/// <summary>
/// Create a touch sensor without communication abilities
/// </summary>
@ -18,13 +19,14 @@ namespace RoboidControl {
/// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
/// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
public TouchSensor(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.TouchSensor, thingId, invokeEvent) { }
*/
/// <summary>
/// Create a new child touch sensor
/// </summary>
/// <param name="parent">The parent thing</param>
/// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
/// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
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 {
/// <returns>A byte array with the binary data</returns>
/// <remark>The byte array will be empty when the touch status has not changed</remark>
public override byte[] GenerateBinary() {
if (this.component == null || !touchUpdated)
if (!touchUpdated)
return Array.Empty<byte>();
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;
/// <summary>
/// Function used to process binary data received for this touch sensor
/// </summary>
/// <param name="bytes">The binary data to process</param>
public override void ProcessBinary(byte[] bytes) {
this.externalTouch = (bytes[0] == 1);
this.touchedSomething |= (bytes[0] == 1);
}
}
}

2
test/UnitTest1.cs
View File

@ -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"
};