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

AR mode works, but reaction time is slow

Browse Source
This commit is contained in:
Pascal Serrarens 2025-05-21 15:59:29 +02:00
parent c4c01bedae
commit 66c58d25f5
14 changed files with 172 additions and 127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
Unity/DifferentialDrive.cs
View File

@ -42,6 +42,8 @@ namespace RoboidControl.Unity {
return differentialDrive; return differentialDrive;
} }
public Motor leftMotor;
public Motor rightMotor;
/// <summary> /// <summary>
/// The left wheel of the differential drive /// The left wheel of the differential drive
/// </summary> /// </summary>
@ -71,41 +73,8 @@ namespace RoboidControl.Unity {
/// Handle the binary event indicating a configuration change /// Handle the binary event indicating a configuration change
/// </summary> /// </summary>
protected override void HandleBinary() { protected override void HandleBinary() {
// Ignore it when the wheel radius is not set HandleWheelBinary(coreDrive.leftWheel, ref leftMotor, ref leftWheel);
if (coreDrive.wheelRadius <= 0) HandleWheelBinary(coreDrive.rightWheel, ref rightMotor, ref rightWheel);
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) { if (casterWheel == null) {
GameObject gameObj = new("Caster wheel"); GameObject gameObj = new("Caster wheel");
@ -113,11 +82,30 @@ namespace RoboidControl.Unity {
casterWheel = gameObj.AddComponent<SphereCollider>(); casterWheel = gameObj.AddComponent<SphereCollider>();
casterWheel.material = Wheel.slidingWheel; casterWheel.material = Wheel.slidingWheel;
} }
casterWheel.radius = coreDrive.wheelRadius; if (coreDrive.wheelRadius > 0 && coreDrive.leftWheel != null && coreDrive.rightWheel != null) {
// Put it in the middle of the back casterWheel.radius = coreDrive.wheelRadius;
// This code assumes that the left wheel position has Direction.left and the right wheel Direction.right... // Put it in the middle of the back
float wheelSeparation = coreDrive.leftWheel.position.distance + coreDrive.rightWheel.position.distance; // This code assumes that the left wheel position has Direction.left and the right wheel Direction.right...
casterWheel.center = new Vector3(0, 0, -wheelSeparation); 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);
}
} }
/// <summary> /// <summary>
@ -126,18 +114,19 @@ namespace RoboidControl.Unity {
protected override void FixedUpdate() { protected override void FixedUpdate() {
base.FixedUpdate(); base.FixedUpdate();
if (rb != null && leftWheel != null && rightWheel != null) { if (rb != null && leftMotor != null && rightMotor != null) {
float leftWheelVelocity = leftWheel.rotationSpeed * (2 * Mathf.PI) * coreDrive.wheelRadius; float leftWheelVelocity = leftMotor.rotationSpeed * (2 * Mathf.PI) * leftWheel.wheelRadius;
float rightWheelVelocity = rightWheel.rotationSpeed * (2 * Mathf.PI) * coreDrive.wheelRadius; float rightWheelVelocity = rightMotor.rotationSpeed * (2 * Mathf.PI) * rightWheel.wheelRadius;
// This code assumes that the left wheel position has Direction.left and the right wheel Direction.right... if (leftWheel != null && rightWheel != null) {
float wheelSeparation = coreDrive.leftWheel.position.distance + coreDrive.rightWheel.position.distance; float wheelSeparation = Vector3.Distance(leftWheel.transform.position, rightWheel.transform.position);
float forwardSpeed = (leftWheelVelocity + rightWheelVelocity) / 2f; float forwardSpeed = (leftWheelVelocity + rightWheelVelocity) / 2f;
float turningSpeed = (leftWheelVelocity - rightWheelVelocity) / wheelSeparation; float turningSpeed = (leftWheelVelocity - rightWheelVelocity) / wheelSeparation;
// Use smoothing to emulate motor inertia // Use smoothing to emulate motor inertia
rb.velocity = 0.9f * rb.velocity + 0.1f * forwardSpeed * transform.forward; rb.velocity = 0.9f * rb.velocity + 0.1f * forwardSpeed * transform.forward;
rb.angularVelocity = 0.9f * rb.angularVelocity + 0.1f * turningSpeed * Vector3.up; rb.angularVelocity = 0.9f * rb.angularVelocity + 0.1f * turningSpeed * Vector3.up;
}
} }
} }
} }

6
Unity/Motor.cs
View File

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

22
Unity/Participant.cs
View File

@ -8,6 +8,16 @@ namespace RoboidControl.Unity {
public RoboidControl.Participant coreParticipant; 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() { protected virtual void Update() {
if (coreParticipant == null) if (coreParticipant == null)
return; return;
@ -36,12 +46,12 @@ namespace RoboidControl.Unity {
DifferentialDrive differentialDrive = DifferentialDrive.Create(coreDrive); DifferentialDrive differentialDrive = DifferentialDrive.Create(coreDrive);
coreDrive.component = differentialDrive; coreDrive.component = differentialDrive;
break; break;
// case RoboidControl.Motor coreMotor: case RoboidControl.Motor coreMotor:
// //Wheel wheel = Wheel.Create(coreMotor); Motor wheel = Motor.Create(coreMotor);
// //coreMotor.component = wheel; coreMotor.component = wheel;
// // We need to know the details (though a binary msg) // We need to know the details (though a binary msg)
// // before we can create the wheel reliably // before we can create the wheel reliably
// break; break;
case RoboidControl.Thing coreThing: case RoboidControl.Thing coreThing:
Debug.Log("Handle Thing"); Debug.Log("Handle Thing");
if (coreThing.component == null) { if (coreThing.component == null) {

24
Unity/Resources/DifferentialDrive_disabled.prefab → Unity/Resources/DifferentialDrive.prefab
View File

@ -101,7 +101,7 @@ BoxCollider:
m_LayerOverridePriority: 0 m_LayerOverridePriority: 0
m_IsTrigger: 0 m_IsTrigger: 0
m_ProvidesContacts: 0 m_ProvidesContacts: 0
m_Enabled: 0 m_Enabled: 1
serializedVersion: 3 serializedVersion: 3
m_Size: {x: 1, y: 1, z: 1} m_Size: {x: 1, y: 1, z: 1}
m_Center: {x: -3.4930155e-36, y: 0, z: 0} m_Center: {x: -3.4930155e-36, y: 0, z: 0}
@ -156,7 +156,7 @@ SphereCollider:
m_ProvidesContacts: 0 m_ProvidesContacts: 0
m_Enabled: 1 m_Enabled: 1
serializedVersion: 3 serializedVersion: 3
m_Radius: 0.00001 m_Radius: 0.025
m_Center: {x: 0, y: 0, z: 0} m_Center: {x: 0, y: 0, z: 0}
--- !u!1 &3377575892836316963 --- !u!1 &3377575892836316963
GameObject: GameObject:
@ -204,9 +204,8 @@ MonoBehaviour:
m_Script: {fileID: 11500000, guid: ddd5065d5e866894cbcb569c3a898ccb, type: 3} m_Script: {fileID: 11500000, guid: ddd5065d5e866894cbcb569c3a898ccb, type: 3}
m_Name: m_Name:
m_EditorClassIdentifier: m_EditorClassIdentifier:
participant: {fileID: 0} owner: {fileID: 0}
maxSpeed: 5 wheelRadius: 0.025
rotationSpeed: 0
--- !u!135 &3720747953092717687 --- !u!135 &3720747953092717687
SphereCollider: SphereCollider:
m_ObjectHideFlags: 0 m_ObjectHideFlags: 0
@ -226,7 +225,7 @@ SphereCollider:
m_ProvidesContacts: 0 m_ProvidesContacts: 0
m_Enabled: 1 m_Enabled: 1
serializedVersion: 3 serializedVersion: 3
m_Radius: 0.00001 m_Radius: 0.025
m_Center: {x: 0, y: 0, z: 0} m_Center: {x: 0, y: 0, z: 0}
--- !u!1 &4326386140118987503 --- !u!1 &4326386140118987503
GameObject: GameObject:
@ -240,7 +239,7 @@ GameObject:
- component: {fileID: 2975045340952151157} - component: {fileID: 2975045340952151157}
- component: {fileID: 4118868690347991998} - component: {fileID: 4118868690347991998}
m_Layer: 0 m_Layer: 0
m_Name: DifferentialDrive_disabled m_Name: DifferentialDrive
m_TagString: Untagged m_TagString: Untagged
m_Icon: {fileID: 0} m_Icon: {fileID: 0}
m_NavMeshLayer: 0 m_NavMeshLayer: 0
@ -277,7 +276,9 @@ MonoBehaviour:
m_Script: {fileID: 11500000, guid: 5c80bc2fd1d8aa14facb1ad4b6ccf7b3, type: 3} m_Script: {fileID: 11500000, guid: 5c80bc2fd1d8aa14facb1ad4b6ccf7b3, type: 3}
m_Name: m_Name:
m_EditorClassIdentifier: m_EditorClassIdentifier:
participant: {fileID: 0} owner: {fileID: 0}
leftMotor: {fileID: 0}
rightMotor: {fileID: 0}
leftWheel: {fileID: 6773972788910618332} leftWheel: {fileID: 6773972788910618332}
rightWheel: {fileID: 7425139233737877139} rightWheel: {fileID: 7425139233737877139}
casterWheel: {fileID: 894333789048616995} casterWheel: {fileID: 894333789048616995}
@ -437,9 +438,8 @@ MonoBehaviour:
m_Script: {fileID: 11500000, guid: ddd5065d5e866894cbcb569c3a898ccb, type: 3} m_Script: {fileID: 11500000, guid: ddd5065d5e866894cbcb569c3a898ccb, type: 3}
m_Name: m_Name:
m_EditorClassIdentifier: m_EditorClassIdentifier:
participant: {fileID: 0} owner: {fileID: 0}
maxSpeed: 5 wheelRadius: 0.025
rotationSpeed: 0
--- !u!135 &8150853676422519226 --- !u!135 &8150853676422519226
SphereCollider: SphereCollider:
m_ObjectHideFlags: 0 m_ObjectHideFlags: 0
@ -459,7 +459,7 @@ SphereCollider:
m_ProvidesContacts: 0 m_ProvidesContacts: 0
m_Enabled: 1 m_Enabled: 1
serializedVersion: 3 serializedVersion: 3
m_Radius: 0.00001 m_Radius: 0.025
m_Center: {x: 0, y: 0, z: 0} m_Center: {x: 0, y: 0, z: 0}
--- !u!1 &5663058822615269368 --- !u!1 &5663058822615269368
GameObject: GameObject:

15
Unity/Thing.cs
View File

@ -46,8 +46,13 @@ namespace RoboidControl.Unity {
protected void Init(RoboidControl.Thing core) { protected void Init(RoboidControl.Thing core) {
this.core = core; this.core = core;
this.core.component = this; this.core.component = this;
this.owner = FindAnyObjectByType<SiteServer>(); Participant[] participants = FindObjectsByType<Participant>(FindObjectsSortMode.None);
core.owner = this.owner.coreParticipant; 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;
if (core.parent != null && core.parent.component != null) { if (core.parent != null && core.parent.component != null) {
this.transform.SetParent(core.parent.component.transform, false); this.transform.SetParent(core.parent.component.transform, false);
@ -90,8 +95,8 @@ namespace RoboidControl.Unity {
/// </summary> /// </summary>
public void UpdateThing() { public void UpdateThing() {
if (core == null) { if (core == null) {
Debug.Log($"{this} core thing is gone, self destruct in 0 seconds..."); //Debug.Log($"{this} core thing is gone, self destruct in 0 seconds...");
Destroy(this); //Destroy(this);
return; return;
} }
@ -129,7 +134,7 @@ namespace RoboidControl.Unity {
this.HandlePose(); this.HandlePose();
break; break;
case BinaryMsg.Id: case BinaryMsg.Id:
Debug.Log($"{this.core.id} Handle Binary"); // Debug.Log($"{this.core.id} Handle Binary");
this.HandleBinary(); this.HandleBinary();
break; break;
} }

36
Unity/TouchSensor.cs
View File

@ -45,6 +45,8 @@ namespace RoboidControl.Unity {
return touchSensor; return touchSensor;
} }
public bool touchedSomething = false;
/// <summary> /// <summary>
/// Handle touch trigger collider enter event /// Handle touch trigger collider enter event
/// </summary> /// </summary>
@ -52,22 +54,40 @@ namespace RoboidControl.Unity {
private void OnTriggerEnter(Collider other) { private void OnTriggerEnter(Collider other) {
// Don't detect trigger colliders // Don't detect trigger colliders
if (other.isTrigger) if (other.isTrigger)
return; return;
// Don't touch yourself // Don't touch yourself
if (this.transform.root == other.transform.root) if (this.transform.root == other.transform.root)
return; return;
Debug.Log("TOUCH!");
this.touchedSomething = true;
this.coreSensor.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));
} }
/// <summary>
/// Handl touch trigger collider exit event // private void OnTriggerStay(Collider other) {
/// </summary> // // Don't detect trigger colliders
/// <param name="other">The collider which exited our trigger collider </param> // if (other.isTrigger)
private void OnTriggerExit(Collider other) { // 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) {
if (other.isTrigger) if (other.isTrigger)
return; return;
Debug.Log("TOUCH END!");
this.touchedSomething = false;
this.coreSensor.touchedSomething = false; this.coreSensor.touchedSomething = false;
} }
} }

6
Unity/Wheel.cs
View File

@ -6,13 +6,13 @@ namespace RoboidControl.Unity {
/// <summary> /// <summary>
/// The Unity representation of a Roboid Control wheel /// The Unity representation of a Roboid Control wheel
/// </summary> /// </summary>
public class Wheel : Motor { public class Wheel : Thing {
/// <summary> /// <summary>
/// Create the Unity representation /// Create the Unity representation
/// </summary> /// </summary>
/// <param name="core">The core motor</param> /// <param name="core">The core motor</param>
/// <returns>The Unity representation of a motorised wheel</returns> /// <returns>The Unity representation of a motorised wheel</returns>
public static Wheel Create(RoboidControl.Motor core, float wheelRadius) { public static new Wheel Create(RoboidControl.Thing core) {
GameObject prefab = (GameObject)Resources.Load("Wheel"); GameObject prefab = (GameObject)Resources.Load("Wheel");
if (prefab != null) { if (prefab != null) {
// Use resource prefab when available // Use resource prefab when available
@ -62,6 +62,8 @@ namespace RoboidControl.Unity {
} }
} }
public float wheelRadius = 0;
private static PhysicMaterial _slidingWheel; private static PhysicMaterial _slidingWheel;
public static PhysicMaterial slidingWheel { public static PhysicMaterial slidingWheel {
get { get {

2
src/Messages/BinaryMsg.cs
View File

@ -72,7 +72,7 @@ namespace RoboidControl {
return 0; return 0;
#if DEBUG #if DEBUG
System.Console.WriteLine($"Send BinaryMsg [{this.networkId}/{this.thingId}] {this.dataLength}"); // System.Console.WriteLine($"Send BinaryMsg [{this.networkId}/{this.thingId}] {this.dataLength}");
#endif #endif
byte ix = 0; byte ix = 0;
buffer[ix++] = BinaryMsg.Id; buffer[ix++] = BinaryMsg.Id;

4
src/Participant.cs
View File

@ -120,9 +120,9 @@ namespace RoboidControl {
return true; return true;
IPEndPoint participantEndpoint = new IPEndPoint(IPAddress.Parse(this.ipAddress), this.port); IPEndPoint participantEndpoint = new IPEndPoint(IPAddress.Parse(this.ipAddress), this.port);
Console.WriteLine($"msg to remote participant {participantEndpoint.Address.ToString()} {participantEndpoint.Port}"); // Console.WriteLine($"msg to remote participant {participantEndpoint.Address.ToString()} {participantEndpoint.Port}");
if (udpClient != null) { if (udpClient != null) {
Console.WriteLine("sending..."); //Console.WriteLine("sending...");
this.udpClient?.Send(this.buffer, bufferSize, participantEndpoint); this.udpClient?.Send(this.buffer, bufferSize, participantEndpoint);
} }
return true; return true;

73
src/Participants/ParticipantUDP.cs
View File

@ -113,7 +113,8 @@ namespace RoboidControl {
/// <summary> /// <summary>
/// The interval in milliseconds for publishing (broadcasting) data on the local network /// The interval in milliseconds for publishing (broadcasting) data on the local network
/// </summary> /// </summary>
public ulong publishInterval = 3000; // = 3 seconds public ulong publishIntervalMS = 3000; // = 3 seconds
public ulong sendUpdateIntervalMS = 100; // for object updates
//public byte[] buffer = new byte[1024]; //public byte[] buffer = new byte[1024];
@ -145,21 +146,22 @@ namespace RoboidControl {
#region Update #region Update
protected ulong nextPublishMe = 0; protected ulong nextPublishMe = 0;
protected ulong nextSendUpdate = 0;
public override void Update(ulong currentTimeMS = 0) { public override void Update(ulong currentTimeMS = 0) {
if (currentTimeMS == 0) if (currentTimeMS == 0)
currentTimeMS = Thing.GetTimeMs(); currentTimeMS = Thing.GetTimeMs();
if (this.isIsolated == false) { if (this.isIsolated == false) {
if (this.publishInterval > 0 && currentTimeMS > this.nextPublishMe) { if (this.publishIntervalMS > 0 && currentTimeMS > this.nextPublishMe) {
ParticipantMsg msg = new ParticipantMsg(this.networkId); ParticipantMsg msg = new(this.networkId);
if (this.remoteSite == null) if (this.remoteSite == null)
this.Publish(msg); this.Publish(msg);
else else
//this.Send(this.remoteSite, msg); //this.Send(this.remoteSite, msg);
this.remoteSite.Send(msg); this.remoteSite.Send(msg);
this.nextPublishMe = currentTimeMS + this.publishInterval; this.nextPublishMe = currentTimeMS + this.publishIntervalMS;
} }
} }
@ -214,17 +216,20 @@ namespace RoboidControl {
if (this.isIsolated) if (this.isIsolated)
continue; continue;
for (int thingIx = 0; thingIx < participant.things.Count; thingIx++) { if (currentTimeMS > this.nextSendUpdate) {
Thing thing = participant.things[thingIx]; for (int thingIx = 0; thingIx < participant.things.Count; thingIx++) {
if (thing == null) Thing thing = participant.things[thingIx];
continue; if (thing == null)
continue;
// PoseMsg poseMsg = new(thing.owner.networkId, thing); // PoseMsg poseMsg = new(thing.owner.networkId, thing);
// this.Send(participant, poseMsg); // this.Send(participant, poseMsg);
// BinaryMsg binaryMsg = new(thing.owner.networkId, thing); // BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
// this.Send(participant, binaryMsg); // this.Send(participant, binaryMsg);
participant.Send(new PoseMsg(thing.owner.networkId, thing)); participant.Send(new PoseMsg(thing.owner.networkId, thing));
participant.Send(new BinaryMsg(thing.owner.networkId, thing)); participant.Send(new BinaryMsg(thing.owner.networkId, thing));
}
this.nextSendUpdate = currentTimeMS + this.sendUpdateIntervalMS;
} }
} }
@ -283,26 +288,32 @@ namespace RoboidControl {
#region Receive #region Receive
protected void ReceiveUDP(IAsyncResult result) { protected void ReceiveUDP(IAsyncResult result) {
// UnityEngine.Debug.Log("received"); try {
if (this.udpClient == null) // || this.endPoint == null) // UnityEngine.Debug.Log("received");
return; if (this.udpClient == null) // || this.endPoint == null)
return;
byte[] data = this.udpClient.EndReceive(result, ref endPoint); byte[] data = this.udpClient.EndReceive(result, ref endPoint);
// This does not yet take multi-packet messages into account! // This does not yet take multi-packet messages into account!
if (endPoint == null) if (endPoint == null)
return; return;
// We can receive our own publish (broadcast) packages. How do we recognize them???? // We can receive our own publish (broadcast) packages. How do we recognize them????
// It is hard to determine our source port // It is hard to determine our source port
string ipAddress = endPoint.Address.ToString(); string ipAddress = endPoint.Address.ToString();
if (ipAddress != this.ipAddress) { if (ipAddress != this.ipAddress) {
Participant remoteParticipant = GetParticipant(ipAddress, endPoint.Port); Participant remoteParticipant = GetParticipant(ipAddress, endPoint.Port);
remoteParticipant ??= AddParticipant(ipAddress, endPoint.Port, this); remoteParticipant ??= AddParticipant(ipAddress, endPoint.Port, this);
ReceiveData(data, remoteParticipant); ReceiveData(data, remoteParticipant);
}
udpClient.BeginReceive(new AsyncCallback(callbackResult => ReceiveUDP(callbackResult)), null);
}
catch (Exception e) {
Console.WriteLine($"Exception in communication thread: {e.Message}");
Console.WriteLine($"Stack trace: {e.StackTrace}");
} }
udpClient.BeginReceive(new AsyncCallback(callbackResult => ReceiveUDP(callbackResult)), null);
} }
public void ReceiveData(byte[] data, Participant sender) { public void ReceiveData(byte[] data, Participant sender) {
@ -424,7 +435,7 @@ namespace RoboidControl {
protected virtual void Process(Participant sender, BinaryMsg msg) { protected virtual void Process(Participant sender, BinaryMsg msg) {
#if DEBUG #if DEBUG
Console.WriteLine($"{this.name}: Process BinaryMsg [{msg.networkId}/{msg.thingId}] {msg.dataLength}"); // Console.WriteLine($"{this.name}: Process BinaryMsg [{msg.networkId}/{msg.thingId}] {msg.dataLength}");
#endif #endif
Thing thing = sender.Get(msg.thingId); Thing thing = sender.Get(msg.thingId);
thing?.ProcessBinary(msg.data); thing?.ProcessBinary(msg.data);

1
src/Participants/SiteServer.cs
View File

@ -134,6 +134,7 @@ namespace RoboidControl {
return msg.thingType switch { return msg.thingType switch {
Thing.Type.TouchSensor => new TouchSensor(sender, msg.thingId), Thing.Type.TouchSensor => new TouchSensor(sender, msg.thingId),
Thing.Type.DifferentialDrive => new DifferentialDrive(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), _ => new Thing(sender, msg.thingType, msg.thingId),
}; };

3
src/Things/DifferentialDrive.cs
View File

@ -160,7 +160,8 @@ namespace RoboidControl {
public override void ProcessBinary(byte[] data) { public override void ProcessBinary(byte[] data) {
byte ix = 0; byte ix = 0;
byte leftWheelId = data[ix++]; byte leftWheelId = data[ix++];
this.leftWheel = this.owner.Get(leftWheelId) as Motor; Thing leftMotor = this.owner.Get(leftWheelId);
this.leftWheel = leftMotor as Motor;
byte rightWheelId = data[ix++]; byte rightWheelId = data[ix++];
this.rightWheel = this.owner.Get(rightWheelId) as Motor; this.rightWheel = this.owner.Get(rightWheelId) as Motor;
this._wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix); this._wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix);

4
src/Things/Motor.cs
View File

@ -5,6 +5,8 @@ namespace RoboidControl {
public class Motor : Thing { public class Motor : Thing {
public Motor(bool invokeEvent = true) : base(Type.UncontrolledMotor, invokeEvent) { } 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(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) { }
/// @brief Motor turning direction /// @brief Motor turning direction
public enum Direction { public enum Direction {
@ -38,7 +40,7 @@ namespace RoboidControl {
return data; return data;
} }
public override void ProcessBinary(byte[] data) { public override void ProcessBinary(byte[] data) {
this.targetSpeed = (float)data[0] / 127; this.targetSpeed = (float)(sbyte)data[0] / 127;
} }
} }

14
src/Things/TouchSensor.cs
View File

@ -35,9 +35,10 @@ namespace RoboidControl {
public bool touchedSomething { public bool touchedSomething {
get { return _touchedSomething; } get { return _touchedSomething; }
set { set {
if (_touchedSomething != value) { //if (_touchedSomething != value) {
_touchedSomething = value; _touchedSomething = value;
} owner.Send(new BinaryMsg(this));
//}
touchUpdated = true; touchUpdated = true;
} }
} }
@ -57,21 +58,24 @@ namespace RoboidControl {
/// <returns>A byte array with the binary data</returns> /// <returns>A byte array with the binary data</returns>
/// <remark>The byte array will be empty when the touch status has not changed</remark> /// <remark>The byte array will be empty when the touch status has not changed</remark>
public override byte[] GenerateBinary() { public override byte[] GenerateBinary() {
if (!touchUpdated) if (this.component == null || !touchUpdated)
return Array.Empty<byte>(); return Array.Empty<byte>();
Unity.TouchSensor touchComponent = this.component as Unity.TouchSensor;
byte[] bytes = new byte[1]; byte[] bytes = new byte[1];
bytes[0] = (byte)(touchedSomething ? 1 : 0); bytes[0] = (byte)(touchComponent.touchedSomething ? 1 : 0);
touchUpdated = false; touchUpdated = false;
Console.WriteLine($"Sending touch {bytes[0]}");
return bytes; return bytes;
} }
private bool externalTouch = false;
/// <summary> /// <summary>
/// Function used to process binary data received for this touch sensor /// Function used to process binary data received for this touch sensor
/// </summary> /// </summary>
/// <param name="bytes">The binary data to process</param> /// <param name="bytes">The binary data to process</param>
public override void ProcessBinary(byte[] bytes) { public override void ProcessBinary(byte[] bytes) {
this.touchedSomething |= (bytes[0] == 1); this.externalTouch = (bytes[0] == 1);
} }
} }
} }