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(float f) {
_value = f32tof16(f);
//_value = f32tof16(f);
_value = F32ToF16__(f);
}
public float toFloat() {
@ -206,6 +207,59 @@ namespace Passer.LinearAlgebra {
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) {
//uint t = *(uint*)&f;
//uint t = (uint)BitConverter.SingleToInt32Bits(f);

2
Messages/BinaryMsg.cs
View File

@ -50,6 +50,8 @@ namespace RoboidControl {
this.thingId = thing.id;
this.thing = thing;
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)
public BinaryMsg(byte[] buffer) {

13
Messages/LowLevelMessages.cs
View File

@ -9,7 +9,7 @@ namespace RoboidControl
{
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.vertical);
}
public static Spherical ReceiveSpherical(byte[] data, ref byte ix)
@ -73,7 +73,8 @@ namespace RoboidControl
angle -= 360;
while (angle < -180)
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)
@ -92,13 +93,15 @@ namespace RoboidControl
public static void SendFloat16(byte[] data, ref byte ix, float16 f)
{
ushort binary = f.GetBinary();
data[ix++] = (byte)(binary >> 8);
data[ix++] = (byte)(binary & 255);
data[ix++] = (byte)((binary >> 8) & 0xFF);
data[ix++] = (byte)(binary & 0xFF);
}
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();
f16.SetBinary(value);
float f = f16.toFloat();

87
Participant.cs
View File

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

8
Sensors/TouchSensor.cs
View File

@ -18,9 +18,11 @@ namespace RoboidControl {
get { return _touchedSomething; }
set {
_touchedSomething = value;
BinaryMsg msg = new(networkId, this);
foreach (RemoteParticipant remoteParticipant in thisParticipant.senders)
thisParticipant.Send(remoteParticipant, msg);
if (thisParticipant != null && this.owner != thisParticipant ) {
BinaryMsg msg = new(networkId, this);
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, 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);
if (thing == null) {
Thing newThing = null;
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)
newThing = msgProcessor(sender, msg.networkId, msg.thingId);
}

8
Thing.cs
View File

@ -206,10 +206,11 @@ namespace RoboidControl {
/// <summary>
/// Create a new thing for the given participant
/// </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>
public Thing(RemoteParticipant participant, bool invokeEvent = false) {
this.owner = participant;
public Thing(RemoteParticipant owner, bool invokeEvent = false) {
this.owner = owner;
//owner.Add(this);
if (invokeEvent)
InvokeNewThing(this);
}
@ -297,6 +298,7 @@ namespace RoboidControl {
if (thing == null)
return false;
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() {
Console.SetOut(new UnityLogWriter());
site = new(7681);
site = new RoboidControl.SiteServer(7681);
RoboidControl.Thing.OnNewThing += HandleNewThing;
//site.Register<RoboidControl.TouchSensor>(RoboidControl.Thing.Type.TouchSensor);
}
void OnApplicationQuit() {
@ -24,7 +22,7 @@ namespace RoboidControl.Unity {
}
public void HandleNewThing(RoboidControl.Thing thing) {
// Debug.Log("Handle New thing event");
//Debug.Log("Handle New thing event");
site.Add(thing, false);
thingQueue.Enqueue(thing);
}

2
Unity/Thing.cs
View File

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

13
Unity/TouchSensor.cs
View File

@ -24,6 +24,18 @@ namespace RoboidControl.Unity {
participant = FindAnyObjectByType<SiteServer>();
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>
@ -62,7 +74,6 @@ namespace RoboidControl.Unity {
if (this.transform.root == other.transform.root)
return;
// Debug.Log($"touched {other.gameObject.name}");
this.coreSensor.touchedSomething = true;
}
private void OnTriggerExit(Collider other) {