From 66c58d25f53d0ccb8d42edefa36b5e707206c3bb Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 21 May 2025 15:59:29 +0200 Subject: [PATCH] AR mode works, but reaction time is slow --- Unity/DifferentialDrive.cs | 89 ++++++++----------- Unity/Motor.cs | 6 +- Unity/Participant.cs | 22 +++-- ...sabled.prefab => DifferentialDrive.prefab} | 24 ++--- Unity/Thing.cs | 15 ++-- Unity/TouchSensor.cs | 36 ++++++-- Unity/Wheel.cs | 6 +- src/Messages/BinaryMsg.cs | 2 +- src/Participant.cs | 4 +- src/Participants/ParticipantUDP.cs | 73 ++++++++------- src/Participants/SiteServer.cs | 1 + src/Things/DifferentialDrive.cs | 3 +- src/Things/Motor.cs | 4 +- src/Things/TouchSensor.cs | 14 +-- 14 files changed, 172 insertions(+), 127 deletions(-) rename Unity/Resources/{DifferentialDrive_disabled.prefab => DifferentialDrive.prefab} (98%) diff --git a/Unity/DifferentialDrive.cs b/Unity/DifferentialDrive.cs index 4f573aa..3ea68bf 100644 --- a/Unity/DifferentialDrive.cs +++ b/Unity/DifferentialDrive.cs @@ -42,6 +42,8 @@ namespace RoboidControl.Unity { return differentialDrive; } + public Motor leftMotor; + public Motor rightMotor; /// /// The left wheel of the differential drive /// @@ -71,41 +73,8 @@ namespace RoboidControl.Unity { /// Handle the binary event indicating a configuration change /// protected override void HandleBinary() { - // Ignore it when the wheel radius is not set - if (coreDrive.wheelRadius <= 0) - return; - - // Destroy any (generic) thing with the same id - if (leftWheel == null) { - Thing[] things = FindObjectsOfType(); - foreach (Thing thing in things) { - if (thing.core.id == coreDrive.leftWheel.id) - Destroy(thing.gameObject); - } - } - if (leftWheel == null && coreDrive.leftWheel != null) - leftWheel = Wheel.Create(this.transform, coreDrive.leftWheel.id); - if (leftWheel != null) { - leftWheel.core ??= coreDrive.leftWheel; - SphereCollider leftWheelCollider = leftWheel.GetComponent(); - leftWheelCollider.radius = coreDrive.wheelRadius; - } - - // Destroy any (generic) thing with the same id - if (rightWheel == null) { - Thing[] things = FindObjectsOfType(); - foreach (Thing thing in things) { - if (thing.core.id == coreDrive.rightWheel.id) - Destroy(thing.gameObject); - } - } - if (rightWheel == null && coreDrive.rightWheel != null) - rightWheel = Wheel.Create(this.transform, coreDrive.rightWheel.id); - if (rightWheel != null) { - rightWheel.core ??= coreDrive.rightWheel; - SphereCollider rightWheelCollider = rightWheel.GetComponent(); - rightWheelCollider.radius = coreDrive.wheelRadius; - } + HandleWheelBinary(coreDrive.leftWheel, ref leftMotor, ref leftWheel); + HandleWheelBinary(coreDrive.rightWheel, ref rightMotor, ref rightWheel); if (casterWheel == null) { GameObject gameObj = new("Caster wheel"); @@ -113,11 +82,30 @@ namespace RoboidControl.Unity { casterWheel = gameObj.AddComponent(); casterWheel.material = Wheel.slidingWheel; } - 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); + 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); + } } /// @@ -126,18 +114,19 @@ namespace RoboidControl.Unity { protected override void FixedUpdate() { base.FixedUpdate(); - 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 (rb != null && leftMotor != null && rightMotor != null) { + float leftWheelVelocity = leftMotor.rotationSpeed * (2 * Mathf.PI) * leftWheel.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... - float wheelSeparation = coreDrive.leftWheel.position.distance + coreDrive.rightWheel.position.distance; - float forwardSpeed = (leftWheelVelocity + rightWheelVelocity) / 2f; - float turningSpeed = (leftWheelVelocity - rightWheelVelocity) / wheelSeparation; + 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; - // Use smoothing to emulate motor inertia - rb.velocity = 0.9f * rb.velocity + 0.1f * forwardSpeed * transform.forward; - rb.angularVelocity = 0.9f * rb.angularVelocity + 0.1f * turningSpeed * Vector3.up; + // Use smoothing to emulate motor inertia + rb.velocity = 0.9f * rb.velocity + 0.1f * forwardSpeed * transform.forward; + rb.angularVelocity = 0.9f * rb.angularVelocity + 0.1f * turningSpeed * Vector3.up; + } } } } diff --git a/Unity/Motor.cs b/Unity/Motor.cs index 3c15cdd..72cf128 100644 --- a/Unity/Motor.cs +++ b/Unity/Motor.cs @@ -34,8 +34,8 @@ namespace RoboidControl.Unity { motor = gameObj.AddComponent(); motor.Init(coreMotor); - Rigidbody rb = gameObj.AddComponent(); - rb.isKinematic = true; + // Rigidbody rb = gameObj.AddComponent(); + // rb.isKinematic = true; } return motor; } @@ -43,7 +43,7 @@ namespace RoboidControl.Unity { /// /// The maximum rotation speed of the motor in rotations per second /// - public float maxSpeed = 5; + public float maxSpeed = 2; /// /// The actual rotation speed in rotations per second diff --git a/Unity/Participant.cs b/Unity/Participant.cs index 62834a8..9a04ec5 100644 --- a/Unity/Participant.cs +++ b/Unity/Participant.cs @@ -8,6 +8,16 @@ namespace RoboidControl.Unity { public RoboidControl.Participant coreParticipant; + // public Thing GetThing(RoboidControl.Thing coreThing) { + // // Not efficient or full correct but will do for now + // Thing[] things = FindObjectsOfType(); + // foreach (Thing thing in things) { + // if (thing.core.id == coreThing.id) + // return thing; + // } + // return null; + // } + protected virtual void Update() { if (coreParticipant == null) return; @@ -36,12 +46,12 @@ namespace RoboidControl.Unity { DifferentialDrive differentialDrive = DifferentialDrive.Create(coreDrive); coreDrive.component = differentialDrive; 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.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.Thing coreThing: Debug.Log("Handle Thing"); if (coreThing.component == null) { diff --git a/Unity/Resources/DifferentialDrive_disabled.prefab b/Unity/Resources/DifferentialDrive.prefab similarity index 98% rename from Unity/Resources/DifferentialDrive_disabled.prefab rename to Unity/Resources/DifferentialDrive.prefab index 3f5f12d..74de13f 100644 --- a/Unity/Resources/DifferentialDrive_disabled.prefab +++ b/Unity/Resources/DifferentialDrive.prefab @@ -101,7 +101,7 @@ BoxCollider: m_LayerOverridePriority: 0 m_IsTrigger: 0 m_ProvidesContacts: 0 - m_Enabled: 0 + m_Enabled: 1 serializedVersion: 3 m_Size: {x: 1, y: 1, z: 1} m_Center: {x: -3.4930155e-36, y: 0, z: 0} @@ -156,7 +156,7 @@ SphereCollider: m_ProvidesContacts: 0 m_Enabled: 1 serializedVersion: 3 - m_Radius: 0.00001 + m_Radius: 0.025 m_Center: {x: 0, y: 0, z: 0} --- !u!1 &3377575892836316963 GameObject: @@ -204,9 +204,8 @@ MonoBehaviour: m_Script: {fileID: 11500000, guid: ddd5065d5e866894cbcb569c3a898ccb, type: 3} m_Name: m_EditorClassIdentifier: - participant: {fileID: 0} - maxSpeed: 5 - rotationSpeed: 0 + owner: {fileID: 0} + wheelRadius: 0.025 --- !u!135 &3720747953092717687 SphereCollider: m_ObjectHideFlags: 0 @@ -226,7 +225,7 @@ SphereCollider: m_ProvidesContacts: 0 m_Enabled: 1 serializedVersion: 3 - m_Radius: 0.00001 + m_Radius: 0.025 m_Center: {x: 0, y: 0, z: 0} --- !u!1 &4326386140118987503 GameObject: @@ -240,7 +239,7 @@ GameObject: - component: {fileID: 2975045340952151157} - component: {fileID: 4118868690347991998} m_Layer: 0 - m_Name: DifferentialDrive_disabled + m_Name: DifferentialDrive m_TagString: Untagged m_Icon: {fileID: 0} m_NavMeshLayer: 0 @@ -277,7 +276,9 @@ MonoBehaviour: m_Script: {fileID: 11500000, guid: 5c80bc2fd1d8aa14facb1ad4b6ccf7b3, type: 3} m_Name: m_EditorClassIdentifier: - participant: {fileID: 0} + owner: {fileID: 0} + leftMotor: {fileID: 0} + rightMotor: {fileID: 0} leftWheel: {fileID: 6773972788910618332} rightWheel: {fileID: 7425139233737877139} casterWheel: {fileID: 894333789048616995} @@ -437,9 +438,8 @@ MonoBehaviour: m_Script: {fileID: 11500000, guid: ddd5065d5e866894cbcb569c3a898ccb, type: 3} m_Name: m_EditorClassIdentifier: - participant: {fileID: 0} - maxSpeed: 5 - rotationSpeed: 0 + owner: {fileID: 0} + wheelRadius: 0.025 --- !u!135 &8150853676422519226 SphereCollider: m_ObjectHideFlags: 0 @@ -459,7 +459,7 @@ SphereCollider: m_ProvidesContacts: 0 m_Enabled: 1 serializedVersion: 3 - m_Radius: 0.00001 + m_Radius: 0.025 m_Center: {x: 0, y: 0, z: 0} --- !u!1 &5663058822615269368 GameObject: diff --git a/Unity/Thing.cs b/Unity/Thing.cs index 43476eb..711b910 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -46,8 +46,13 @@ namespace RoboidControl.Unity { protected void Init(RoboidControl.Thing core) { this.core = core; this.core.component = this; - this.owner = FindAnyObjectByType(); - core.owner = this.owner.coreParticipant; + Participant[] participants = FindObjectsByType(FindObjectsSortMode.None); + foreach (Participant participant in participants) { + if (participant.ipAddress == core.owner.ipAddress && participant.port == core.owner.port) + this.owner = participant; + } + //this.owner = FindAnyObjectByType(); + //core.owner = this.owner.coreParticipant; if (core.parent != null && core.parent.component != null) { this.transform.SetParent(core.parent.component.transform, false); @@ -90,8 +95,8 @@ namespace RoboidControl.Unity { /// public void UpdateThing() { if (core == null) { - Debug.Log($"{this} core thing is gone, self destruct in 0 seconds..."); - Destroy(this); + //Debug.Log($"{this} core thing is gone, self destruct in 0 seconds..."); + //Destroy(this); return; } @@ -129,7 +134,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; } diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs index 9702ffb..3e17c46 100644 --- a/Unity/TouchSensor.cs +++ b/Unity/TouchSensor.cs @@ -45,6 +45,8 @@ namespace RoboidControl.Unity { return touchSensor; } + public bool touchedSomething = false; + /// /// Handle touch trigger collider enter event /// @@ -52,22 +54,40 @@ namespace RoboidControl.Unity { private void OnTriggerEnter(Collider other) { // Don't detect trigger colliders if (other.isTrigger) - return; + return; // Don't touch yourself if (this.transform.root == other.transform.root) - return; + return; + Debug.Log("TOUCH!"); + this.touchedSomething = true; this.coreSensor.touchedSomething = true; - this.core.updateQueue.Enqueue(new RoboidControl.Thing.CoreEvent(BinaryMsg.Id)); + //this.core.updateQueue.Enqueue(new RoboidControl.Thing.CoreEvent(BinaryMsg.Id)); } - /// - /// Handl touch trigger collider exit event - /// - /// The collider which exited our trigger collider - private void OnTriggerExit(Collider other) { + + // private void OnTriggerStay(Collider other) { + // // Don't detect trigger colliders + // if (other.isTrigger) + // return; + // // Don't touch yourself + // if (this.transform.root == other.transform.root) + // return; + + // Debug.Log("Touching..."); + // this.touchedSomething = true; + // this.coreSensor.touchedSomething = true; + // } + + /// + /// Handl touch trigger collider exit event + /// + /// The collider which exited our trigger collider + private void OnTriggerExit(Collider other) { if (other.isTrigger) return; + Debug.Log("TOUCH END!"); + this.touchedSomething = false; this.coreSensor.touchedSomething = false; } } diff --git a/Unity/Wheel.cs b/Unity/Wheel.cs index 53b7820..0e52270 100644 --- a/Unity/Wheel.cs +++ b/Unity/Wheel.cs @@ -6,13 +6,13 @@ namespace RoboidControl.Unity { /// /// The Unity representation of a Roboid Control wheel /// - public class Wheel : Motor { + public class Wheel : Thing { /// /// Create the Unity representation /// /// The core motor /// The Unity representation of a motorised wheel - public static Wheel Create(RoboidControl.Motor core, float wheelRadius) { + public static new Wheel Create(RoboidControl.Thing core) { GameObject prefab = (GameObject)Resources.Load("Wheel"); if (prefab != null) { // Use resource prefab when available @@ -62,6 +62,8 @@ namespace RoboidControl.Unity { } } + public float wheelRadius = 0; + private static PhysicMaterial _slidingWheel; public static PhysicMaterial slidingWheel { get { diff --git a/src/Messages/BinaryMsg.cs b/src/Messages/BinaryMsg.cs index c46fa02..c932bae 100644 --- a/src/Messages/BinaryMsg.cs +++ b/src/Messages/BinaryMsg.cs @@ -72,7 +72,7 @@ namespace RoboidControl { return 0; #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 byte ix = 0; buffer[ix++] = BinaryMsg.Id; diff --git a/src/Participant.cs b/src/Participant.cs index 4318467..572ec93 100644 --- a/src/Participant.cs +++ b/src/Participant.cs @@ -120,9 +120,9 @@ namespace RoboidControl { return true; 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) { - Console.WriteLine("sending..."); + //Console.WriteLine("sending..."); this.udpClient?.Send(this.buffer, bufferSize, participantEndpoint); } return true; diff --git a/src/Participants/ParticipantUDP.cs b/src/Participants/ParticipantUDP.cs index 0fde7f2..a133948 100644 --- a/src/Participants/ParticipantUDP.cs +++ b/src/Participants/ParticipantUDP.cs @@ -113,7 +113,8 @@ namespace RoboidControl { /// /// The interval in milliseconds for publishing (broadcasting) data on the local network /// - 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]; @@ -145,21 +146,22 @@ namespace RoboidControl { #region Update protected ulong nextPublishMe = 0; + protected ulong nextSendUpdate = 0; public override void Update(ulong currentTimeMS = 0) { if (currentTimeMS == 0) currentTimeMS = Thing.GetTimeMs(); if (this.isIsolated == false) { - if (this.publishInterval > 0 && currentTimeMS > this.nextPublishMe) { - ParticipantMsg msg = new ParticipantMsg(this.networkId); + if (this.publishIntervalMS > 0 && currentTimeMS > this.nextPublishMe) { + ParticipantMsg msg = new(this.networkId); if (this.remoteSite == null) this.Publish(msg); else //this.Send(this.remoteSite, msg); this.remoteSite.Send(msg); - this.nextPublishMe = currentTimeMS + this.publishInterval; + this.nextPublishMe = currentTimeMS + this.publishIntervalMS; } } @@ -214,17 +216,20 @@ namespace RoboidControl { if (this.isIsolated) continue; - for (int thingIx = 0; thingIx < participant.things.Count; thingIx++) { - Thing thing = participant.things[thingIx]; - if (thing == null) - continue; + if (currentTimeMS > this.nextSendUpdate) { + for (int thingIx = 0; thingIx < participant.things.Count; thingIx++) { + Thing thing = participant.things[thingIx]; + if (thing == null) + continue; - // PoseMsg poseMsg = new(thing.owner.networkId, thing); - // this.Send(participant, poseMsg); - // BinaryMsg binaryMsg = new(thing.owner.networkId, thing); - // this.Send(participant, binaryMsg); - participant.Send(new PoseMsg(thing.owner.networkId, thing)); - participant.Send(new BinaryMsg(thing.owner.networkId, thing)); + // PoseMsg poseMsg = new(thing.owner.networkId, thing); + // this.Send(participant, poseMsg); + // BinaryMsg binaryMsg = new(thing.owner.networkId, thing); + // this.Send(participant, binaryMsg); + participant.Send(new PoseMsg(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 protected void ReceiveUDP(IAsyncResult result) { - // UnityEngine.Debug.Log("received"); - if (this.udpClient == null) // || this.endPoint == null) - return; + try { + // UnityEngine.Debug.Log("received"); + if (this.udpClient == null) // || this.endPoint == null) + return; - byte[] data = this.udpClient.EndReceive(result, ref endPoint); - // This does not yet take multi-packet messages into account! - if (endPoint == null) - return; + byte[] data = this.udpClient.EndReceive(result, ref endPoint); + // This does not yet take multi-packet messages into account! + if (endPoint == null) + return; - // We can receive our own publish (broadcast) packages. How do we recognize them???? - // It is hard to determine our source port - string ipAddress = endPoint.Address.ToString(); - if (ipAddress != this.ipAddress) { - Participant remoteParticipant = GetParticipant(ipAddress, endPoint.Port); - remoteParticipant ??= AddParticipant(ipAddress, endPoint.Port, this); + // We can receive our own publish (broadcast) packages. How do we recognize them???? + // It is hard to determine our source port + string ipAddress = endPoint.Address.ToString(); + if (ipAddress != this.ipAddress) { + Participant remoteParticipant = GetParticipant(ipAddress, endPoint.Port); + 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) { @@ -424,7 +435,7 @@ namespace RoboidControl { protected virtual void Process(Participant sender, BinaryMsg msg) { #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 Thing thing = sender.Get(msg.thingId); thing?.ProcessBinary(msg.data); diff --git a/src/Participants/SiteServer.cs b/src/Participants/SiteServer.cs index 8c38572..53e21c1 100644 --- a/src/Participants/SiteServer.cs +++ b/src/Participants/SiteServer.cs @@ -134,6 +134,7 @@ namespace RoboidControl { 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), }; diff --git a/src/Things/DifferentialDrive.cs b/src/Things/DifferentialDrive.cs index 45203a8..c9b4d00 100644 --- a/src/Things/DifferentialDrive.cs +++ b/src/Things/DifferentialDrive.cs @@ -160,7 +160,8 @@ namespace RoboidControl { public override void ProcessBinary(byte[] data) { byte ix = 0; 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++]; this.rightWheel = this.owner.Get(rightWheelId) as Motor; this._wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix); diff --git a/src/Things/Motor.cs b/src/Things/Motor.cs index 1b09654..4534400 100644 --- a/src/Things/Motor.cs +++ b/src/Things/Motor.cs @@ -5,6 +5,8 @@ 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) { } + /// @brief Motor turning direction public enum Direction { @@ -38,7 +40,7 @@ namespace RoboidControl { return data; } public override void ProcessBinary(byte[] data) { - this.targetSpeed = (float)data[0] / 127; + this.targetSpeed = (float)(sbyte)data[0] / 127; } } diff --git a/src/Things/TouchSensor.cs b/src/Things/TouchSensor.cs index 2198e6e..4443eae 100644 --- a/src/Things/TouchSensor.cs +++ b/src/Things/TouchSensor.cs @@ -35,9 +35,10 @@ namespace RoboidControl { public bool touchedSomething { get { return _touchedSomething; } set { - if (_touchedSomething != value) { + //if (_touchedSomething != value) { _touchedSomething = value; - } + owner.Send(new BinaryMsg(this)); + //} touchUpdated = true; } } @@ -57,21 +58,24 @@ namespace RoboidControl { /// A byte array with the binary data /// The byte array will be empty when the touch status has not changed public override byte[] GenerateBinary() { - if (!touchUpdated) + if (this.component == null || !touchUpdated) return Array.Empty(); + Unity.TouchSensor touchComponent = this.component as Unity.TouchSensor; byte[] bytes = new byte[1]; - bytes[0] = (byte)(touchedSomething ? 1 : 0); + bytes[0] = (byte)(touchComponent.touchedSomething ? 1 : 0); touchUpdated = false; + Console.WriteLine($"Sending touch {bytes[0]}"); return bytes; } + private bool externalTouch = false; /// /// Function used to process binary data received for this touch sensor /// /// The binary data to process public override void ProcessBinary(byte[] bytes) { - this.touchedSomething |= (bytes[0] == 1); + this.externalTouch = (bytes[0] == 1); } } } \ No newline at end of file