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

ESP32 ant following trails

Browse Source
This commit is contained in:
Pascal Serrarens 2025-02-27 11:19:50 +01:00
parent 0d115ee65a
commit 78d0e179df
10 changed files with 131 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
LinearAlgebra/float16.cs
View File

@ -15,7 +15,8 @@ namespace Passer.LinearAlgebra {
public float16() { _value = 0; } public float16() { _value = 0; }
public float16(float f) { public float16(float f) {
_value = f32tof16(f); //_value = f32tof16(f);
_value = F32ToF16__(f);
} }
public float toFloat() { public float toFloat() {
@ -206,6 +207,59 @@ namespace Passer.LinearAlgebra {
return BitConverter.ToUInt32(bytes, 0); return BitConverter.ToUInt32(bytes, 0);
} }
public ushort F32ToF16__(float f) {
uint t = BitConverter.ToUInt32(BitConverter.GetBytes(f), 0);
ushort man = (ushort)((t & 0x007FFFFF) >> 12);
int exp = (int)((t & 0x7F800000) >> 23);
bool sgn = (t & 0x80000000) != 0;
// handle 0
if ((t & 0x7FFFFFFF) == 0) {
return sgn ? (ushort)0x8000 : (ushort)0x0000;
}
// denormalized float32 does not fit in float16
if (exp == 0x00) {
return sgn ? (ushort)0x8000 : (ushort)0x0000;
}
// handle infinity & NAN
if (exp == 0x00FF) {
if (man != 0)
return 0xFE00; // NAN
return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF
}
// normal numbers
exp = exp - 127 + 15;
// overflow does not fit => INF
if (exp > 30) {
return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF
}
// subnormal numbers
if (exp < -38) {
return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ?
}
if (exp <= 0) // subnormal
{
man >>= (exp + 14);
// rounding
man++;
man >>= 1;
if (sgn)
return (ushort)(0x8000 | man);
return man;
}
// normal
// TODO rounding
exp <<= 10;
man++;
man >>= 1;
if (sgn)
return (ushort)(0x8000 | exp | man);
return (ushort)(exp | man);
}
//This function is faulty!!!!
ushort f32tof16(float f) { ushort f32tof16(float f) {
//uint t = *(uint*)&f; //uint t = *(uint*)&f;
//uint t = (uint)BitConverter.SingleToInt32Bits(f); //uint t = (uint)BitConverter.SingleToInt32Bits(f);

2
Messages/BinaryMsg.cs
View File

@ -50,6 +50,8 @@ namespace RoboidControl {
this.thingId = thing.id; this.thingId = thing.id;
this.thing = thing; this.thing = thing;
this.bytes = this.thing.GenerateBinary(); this.bytes = this.thing.GenerateBinary();
// if (this.bytes.Length > 0)
// System.Console.Write($"Binary message for [{networkId}/{thing.id}]");
} }
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer) /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public BinaryMsg(byte[] buffer) { public BinaryMsg(byte[] buffer) {

13
Messages/LowLevelMessages.cs
View File

@ -9,7 +9,7 @@ namespace RoboidControl
{ {
SendFloat16(buffer, ref ix, new float16(v.distance)); SendFloat16(buffer, ref ix, new float16(v.distance));
SendAngle8(buffer, ref ix, v.direction.horizontal); SendAngle8(buffer, ref ix, v.direction.horizontal);
SendAngle8(buffer, ref ix, v.direction.horizontal); SendAngle8(buffer, ref ix, v.direction.vertical);
} }
public static Spherical ReceiveSpherical(byte[] data, ref byte ix) public static Spherical ReceiveSpherical(byte[] data, ref byte ix)
@ -73,7 +73,8 @@ namespace RoboidControl
angle -= 360; angle -= 360;
while (angle < -180) while (angle < -180)
angle += 360; angle += 360;
buffer[ix++] = (byte)((angle / 360.0f) * 256.0f); sbyte value = (sbyte)(angle / 360.0f * 256.0f);
buffer[ix++] = (byte)value;
} }
public static float ReceiveAngle8(byte[] data, ref byte ix) public static float ReceiveAngle8(byte[] data, ref byte ix)
@ -92,13 +93,15 @@ namespace RoboidControl
public static void SendFloat16(byte[] data, ref byte ix, float16 f) public static void SendFloat16(byte[] data, ref byte ix, float16 f)
{ {
ushort binary = f.GetBinary(); ushort binary = f.GetBinary();
data[ix++] = (byte)(binary >> 8); data[ix++] = (byte)((binary >> 8) & 0xFF);
data[ix++] = (byte)(binary & 255); data[ix++] = (byte)(binary & 0xFF);
} }
public static float ReceiveFloat16(byte[] data, ref byte ix) public static float ReceiveFloat16(byte[] data, ref byte ix)
{ {
ushort value = (ushort)(data[ix++] << 8 | data[ix++]); byte msb = data[ix++];
byte lsb = data[ix++];
ushort value = (ushort)(msb << 8 | lsb);
float16 f16 = new float16(); float16 f16 = new float16();
f16.SetBinary(value); f16.SetBinary(value);
float f = f16.toFloat(); float f = f16.toFloat();

87
Participant.cs
View File

@ -66,11 +66,11 @@ namespace RoboidControl {
this.port = port; this.port = port;
} }
public List<RemoteParticipant> senders = new List<RemoteParticipant>(); public List<RemoteParticipant> owners = new List<RemoteParticipant>();
public RemoteParticipant GetParticipant(string ipAddress, int port) { public RemoteParticipant GetParticipant(string ipAddress, int port) {
//Console.WriteLine($"Get Participant {ipAddress}:{port}"); //Console.WriteLine($"Get Participant {ipAddress}:{port}");
foreach (RemoteParticipant sender in senders) { foreach (RemoteParticipant sender in owners) {
if (sender.ipAddress == ipAddress && sender.port == port) if (sender.ipAddress == ipAddress && sender.port == port)
return sender; return sender;
} }
@ -79,9 +79,9 @@ namespace RoboidControl {
public RemoteParticipant AddParticipant(string ipAddress, int port) { public RemoteParticipant AddParticipant(string ipAddress, int port) {
Console.WriteLine($"New Participant {ipAddress}:{port}"); Console.WriteLine($"New Participant {ipAddress}:{port}");
RemoteParticipant participant = new(ipAddress, port) { RemoteParticipant participant = new(ipAddress, port) {
networkId = (byte)(this.senders.Count + 1) networkId = (byte)(this.owners.Count + 1)
}; };
senders.Add(participant); owners.Add(participant);
return participant; return participant;
} }
@ -119,8 +119,7 @@ namespace RoboidControl {
// It is hard to determine our source port // It is hard to determine our source port
string ipAddress = this.endPoint.Address.ToString(); string ipAddress = this.endPoint.Address.ToString();
RemoteParticipant remoteParticipant = GetParticipant(ipAddress, this.endPoint.Port); RemoteParticipant remoteParticipant = GetParticipant(ipAddress, this.endPoint.Port);
if (remoteParticipant == null) remoteParticipant ??= AddParticipant(ipAddress, this.endPoint.Port);
remoteParticipant = AddParticipant(ipAddress, this.endPoint.Port);
ReceiveData(data, remoteParticipant); ReceiveData(data, remoteParticipant);
@ -146,10 +145,10 @@ namespace RoboidControl {
Thing thing = this.things[ix]; Thing thing = this.things[ix];
if (thing != null) { if (thing != null) {
thing.Update(currentTimeMS); thing.Update(currentTimeMS);
BinaryMsg binaryMsg = new(thing.owner.networkId, thing); // if (thing.owner != this) {
this.Send(thing.owner, binaryMsg); // BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
//foreach (RemoteParticipant sender in this.senders) // this.Send(thing.owner, binaryMsg);
// this.Send(sender, binaryMsg); // }
} }
} }
} }
@ -170,16 +169,6 @@ namespace RoboidControl {
this.Send(remoteParticipant, new BinaryMsg(this.networkId, thing)); this.Send(remoteParticipant, new BinaryMsg(this.networkId, thing));
} }
// public bool Send(IMessage msg) {
// int bufferSize = msg.Serialize(ref this.buffer);
// if (bufferSize <= 0)
// return true;
// // Console.WriteLine($"msg to {endPoint.Address.ToString()} {endPoint.Port}");
// this.udpClient?.Send(this.buffer, bufferSize, this.endPoint);
// return true;
// }
public bool Send(RemoteParticipant remoteParticipant, IMessage msg) { public bool Send(RemoteParticipant remoteParticipant, IMessage msg) {
int bufferSize = msg.Serialize(ref this.buffer); int bufferSize = msg.Serialize(ref this.buffer);
if (bufferSize <= 0) if (bufferSize <= 0)
@ -209,23 +198,23 @@ namespace RoboidControl {
return true; return true;
} }
public bool SendBuffer(int bufferSize) { // public bool SendBuffer(int bufferSize) {
//if (this.ipAddress == null) // //if (this.ipAddress == null)
// return false; // // return false;
// UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}"); // // UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}");
//this.udpClient.Send(this.buffer, bufferSize, this.ipAddress, this.port); // //this.udpClient.Send(this.buffer, bufferSize, this.ipAddress, this.port);
this.udpClient?.Send(this.buffer, bufferSize, this.endPoint); // this.udpClient?.Send(this.buffer, bufferSize, this.endPoint);
return true; // return true;
} // }
public bool PublishBuffer(int bufferSize) { // public bool PublishBuffer(int bufferSize) {
if (this.broadcastIpAddress == null) // if (this.broadcastIpAddress == null)
return false; // return false;
this.udpClient?.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port); // this.udpClient?.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port);
return true; // return true;
} // }
#endregion #endregion
@ -290,14 +279,14 @@ namespace RoboidControl {
} }
} }
protected virtual void Process(InvestigateMsg msg) { } protected virtual void Process(RemoteParticipant sender, InvestigateMsg msg) { }
protected virtual void Process(RemoteParticipant sender, ThingMsg msg) { protected virtual void Process(RemoteParticipant sender, ThingMsg msg) {
Console.WriteLine($"Participant: Process thing [{msg.networkId}/{msg.thingId}]"); //Console.WriteLine($"Participant: Process thing [{msg.networkId}/{msg.thingId}]");
} }
protected virtual void Process(RemoteParticipant sender, NameMsg msg) { protected virtual void Process(RemoteParticipant sender, NameMsg msg) {
// Console.WriteLine($"Participant: Process name [{msg.networkId}/{msg.thingId}] {msg.name}"); //Console.WriteLine($"Participant: Process name [{msg.networkId}/{msg.thingId}] {msg.name}");
Thing thing = sender.Get(msg.networkId, msg.thingId); Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null) if (thing != null)
thing.name = msg.name; thing.name = msg.name;
@ -327,9 +316,9 @@ namespace RoboidControl {
thing.linearVelocity = msg.linearVelocity; thing.linearVelocity = msg.linearVelocity;
if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0) if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0)
thing.angularVelocity = msg.angularVelocity; thing.angularVelocity = msg.angularVelocity;
} }
} }
protected virtual void Process(RemoteParticipant sender, BinaryMsg msg) { protected virtual void Process(RemoteParticipant sender, BinaryMsg msg) {
// Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]"); // Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]");
@ -337,19 +326,19 @@ namespace RoboidControl {
thing?.ProcessBinary(msg.bytes); thing?.ProcessBinary(msg.bytes);
} }
protected virtual void Process(TextMsg temsgxt) { } protected virtual void Process(RemoteParticipant sender, TextMsg temsgxt) { }
protected virtual void Process(DestroyMsg msg) { } protected virtual void Process(RemoteParticipant sender, DestroyMsg msg) { }
private void ForwardMessage(IMessage msg) { private void ForwardMessage(IMessage msg) {
foreach (Participant client in senders) { // foreach (Participant client in senders) {
if (client == this) // if (client == this)
continue; // continue;
//UnityEngine.Debug.Log($"---> {client.ipAddress}"); // //UnityEngine.Debug.Log($"---> {client.ipAddress}");
//IMessage.SendMsg(client, msg); // //IMessage.SendMsg(client, msg);
msg.Serialize(ref client.buffer); // msg.Serialize(ref client.buffer);
client.SendBuffer(client.buffer.Length); // client.SendBuffer(client.buffer.Length);
} // }
} }
#endregion #endregion

8
Sensors/TouchSensor.cs
View File

@ -18,9 +18,11 @@ namespace RoboidControl {
get { return _touchedSomething; } get { return _touchedSomething; }
set { set {
_touchedSomething = value; _touchedSomething = value;
BinaryMsg msg = new(networkId, this); if (thisParticipant != null && this.owner != thisParticipant ) {
foreach (RemoteParticipant remoteParticipant in thisParticipant.senders) BinaryMsg msg = new(networkId, this);
thisParticipant.Send(remoteParticipant, msg); foreach (RemoteParticipant remoteParticipant in thisParticipant.owners)
thisParticipant.Send(remoteParticipant, msg);
}
} }
} }

4
SiteServer.cs
View File

@ -52,12 +52,12 @@ namespace RoboidControl {
protected override void Process(RemoteParticipant sender, NetworkIdMsg msg) { } protected override void Process(RemoteParticipant sender, NetworkIdMsg msg) { }
protected override void Process(RemoteParticipant sender, ThingMsg msg) { protected override void Process(RemoteParticipant sender, ThingMsg msg) {
Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]"); //Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]");
Thing thing = sender.Get(msg.networkId, msg.thingId); Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing == null) { if (thing == null) {
Thing newThing = null; Thing newThing = null;
if (thingMsgProcessors.TryGetValue(msg.thingType, out Func<RemoteParticipant, byte, byte, Thing> msgProcessor)) { if (thingMsgProcessors.TryGetValue(msg.thingType, out Func<RemoteParticipant, byte, byte, Thing> msgProcessor)) {
Console.WriteLine("Found thing message processor"); //Console.WriteLine("Found thing message processor");
if (msgProcessor != null) if (msgProcessor != null)
newThing = msgProcessor(sender, msg.networkId, msg.thingId); newThing = msgProcessor(sender, msg.networkId, msg.thingId);
} }

8
Thing.cs
View File

@ -206,10 +206,11 @@ namespace RoboidControl {
/// <summary> /// <summary>
/// Create a new thing for the given participant /// Create a new thing for the given participant
/// </summary> /// </summary>
/// <param name="participant">The participant for which this thing is created</param> /// <param name="owner">The participant for which this thing is created</param>
/// <param name="invokeEvent">True when a new thing event should be triggered</param> /// <param name="invokeEvent">True when a new thing event should be triggered</param>
public Thing(RemoteParticipant participant, bool invokeEvent = false) { public Thing(RemoteParticipant owner, bool invokeEvent = false) {
this.owner = participant; this.owner = owner;
//owner.Add(this);
if (invokeEvent) if (invokeEvent)
InvokeNewThing(this); InvokeNewThing(this);
} }
@ -297,6 +298,7 @@ namespace RoboidControl {
if (thing == null) if (thing == null)
return false; return false;
return (thing.networkId == networkId) && (thing.id == thingId); return (thing.networkId == networkId) && (thing.id == thingId);
//return (thing.id == thingId);
} }
} }

6
Unity/SiteServer.cs
View File

@ -13,10 +13,8 @@ namespace RoboidControl.Unity {
protected virtual void Awake() { protected virtual void Awake() {
Console.SetOut(new UnityLogWriter()); Console.SetOut(new UnityLogWriter());
site = new(7681); site = new RoboidControl.SiteServer(7681);
RoboidControl.Thing.OnNewThing += HandleNewThing; RoboidControl.Thing.OnNewThing += HandleNewThing;
//site.Register<RoboidControl.TouchSensor>(RoboidControl.Thing.Type.TouchSensor);
} }
void OnApplicationQuit() { void OnApplicationQuit() {
@ -24,7 +22,7 @@ namespace RoboidControl.Unity {
} }
public void HandleNewThing(RoboidControl.Thing thing) { public void HandleNewThing(RoboidControl.Thing thing) {
// Debug.Log("Handle New thing event"); //Debug.Log("Handle New thing event");
site.Add(thing, false); site.Add(thing, false);
thingQueue.Enqueue(thing); thingQueue.Enqueue(thing);
} }

2
Unity/Thing.cs
View File

@ -96,6 +96,8 @@ namespace RoboidControl.Unity {
modelQuad.transform.SetParent(this.transform, false); modelQuad.transform.SetParent(this.transform, false);
modelQuad.transform.localEulerAngles = new(90, -90, 0); modelQuad.transform.localEulerAngles = new(90, -90, 0);
modelQuad.transform.localScale = new Vector3(aspectRatio, 1, 1) / 5; modelQuad.transform.localScale = new Vector3(aspectRatio, 1, 1) / 5;
if (this.name == "Ant")
modelQuad.transform.localScale *= 2;
Material quadMaterial = new(Shader.Find("Unlit/Transparent")) { Material quadMaterial = new(Shader.Find("Unlit/Transparent")) {
mainTexture = texture mainTexture = texture
}; };

13
Unity/TouchSensor.cs
View File

@ -24,6 +24,18 @@ namespace RoboidControl.Unity {
participant = FindAnyObjectByType<SiteServer>(); participant = FindAnyObjectByType<SiteServer>();
SetCoreThing(new RoboidControl.TouchSensor(participant.site)); SetCoreThing(new RoboidControl.TouchSensor(participant.site));
} }
// Somehow this does not work.
// Rigidbody rb = GetComponentInParent<Rigidbody>();
// if (rb == null) {
// RoboidControl.Thing thing = core;
// while (thing.parent != null)
// thing = thing.parent;
// Thing unityThing = thing.component;
// rb = unityThing.gameObject.AddComponent<Rigidbody>();
// rb.isKinematic = true;
// }
} }
/// <summary> /// <summary>
@ -62,7 +74,6 @@ namespace RoboidControl.Unity {
if (this.transform.root == other.transform.root) if (this.transform.root == other.transform.root)
return; return;
// Debug.Log($"touched {other.gameObject.name}");
this.coreSensor.touchedSomething = true; this.coreSensor.touchedSomething = true;
} }
private void OnTriggerExit(Collider other) { private void OnTriggerExit(Collider other) {