BB2B with Diff.Drive starts to work
This commit is contained in:
		
							parent
							
								
									7461ae1595
								
							
						
					
					
						commit
						0a9e11294a
					
				| @ -10,8 +10,9 @@ namespace RoboidControl { | |||||||
| 
 | 
 | ||||||
|         public BB2B(Participant owner) : base(owner) { |         public BB2B(Participant owner) : base(owner) { | ||||||
|             this.name = "BB2B"; |             this.name = "BB2B"; | ||||||
|             this.wheelRadius = 0.032f; |             this.SetDriveDimensions(0.064f, 0.128f); | ||||||
|             this.wheelSeparation = 0.128f; |             // this.wheelRadius = 0.032f; | ||||||
|  |             // this.wheelSeparation = 0.128f; | ||||||
| 
 | 
 | ||||||
|             // Is has a touch sensor at the front left of the roboid |             // Is has a touch sensor at the front left of the roboid | ||||||
|             touchLeft = new(this) { |             touchLeft = new(this) { | ||||||
| @ -28,11 +29,11 @@ namespace RoboidControl { | |||||||
| 
 | 
 | ||||||
|             // The left wheel turns forward when nothing is touched on the right side |             // The left wheel turns forward when nothing is touched on the right side | ||||||
|             // and turn backward when the roboid hits something on the right |             // and turn backward when the roboid hits something on the right | ||||||
|             float leftWheelSpeed = touchRight.touchedSomething ? -600.0f : 600.0f; |             float leftWheelSpeed = touchRight.touchedSomething ? -0.1f : 0.1f; | ||||||
| 
 | 
 | ||||||
|             // The right wheel does the same, but instead is controlled by |             // The right wheel does the same, but instead is controlled by | ||||||
|             // touches on the left side |             // touches on the left side | ||||||
|             float rightWheelSpeed = touchLeft.touchedSomething ? -600.0f : 600.0f; |             float rightWheelSpeed = touchLeft.touchedSomething ? -0.1f : 0.1f; | ||||||
| 
 | 
 | ||||||
|             // When both sides are touching something, both wheels will turn backward |             // When both sides are touching something, both wheels will turn backward | ||||||
|             // and the roboid will move backwards |             // and the roboid will move backwards | ||||||
|  | |||||||
| @ -1,12 +1,11 @@ | |||||||
| #if UNITY_5_3_OR_NEWER | #if UNITY_5_3_OR_NEWER | ||||||
| using System.Linq; |  | ||||||
| using UnityEngine; | using UnityEngine; | ||||||
| 
 | 
 | ||||||
| namespace RoboidControl.Unity { | namespace RoboidControl.Unity { | ||||||
|     public class DifferentialDrive : Thing { |     public class DifferentialDrive : Thing { | ||||||
| 
 | 
 | ||||||
|         public WheelCollider leftWheel; |         public Wheel leftWheel; | ||||||
|         public WheelCollider rightWheel; |         public Wheel rightWheel; | ||||||
| 
 | 
 | ||||||
|         /// <summary> |         /// <summary> | ||||||
|         /// Create the Unity representation |         /// Create the Unity representation | ||||||
| @ -15,15 +14,15 @@ namespace RoboidControl.Unity { | |||||||
|         /// <returns>The Unity representation of the touch sensor</returns> |         /// <returns>The Unity representation of the touch sensor</returns> | ||||||
|         public static DifferentialDrive Create(RoboidControl.DifferentialDrive core) { |         public static DifferentialDrive Create(RoboidControl.DifferentialDrive core) { | ||||||
|             GameObject prefab = (GameObject)Resources.Load("DifferentialDrive"); |             GameObject prefab = (GameObject)Resources.Load("DifferentialDrive"); | ||||||
|             if (prefab != null) { |             // if (prefab != null) { | ||||||
|                 // Use resource prefab when available |             //     // Use resource prefab when available | ||||||
|                 GameObject gameObj = Instantiate(prefab); |             //     GameObject gameObj = Instantiate(prefab); | ||||||
|                 DifferentialDrive component = gameObj.GetComponent<DifferentialDrive>(); |             //     DifferentialDrive component = gameObj.GetComponent<DifferentialDrive>(); | ||||||
|                 if (component != null) |             //     if (component != null) | ||||||
|                     component.core = core; |             //         component.core = core; | ||||||
|                 return component; |             //     return component; | ||||||
|             } |             // } | ||||||
|             else { |             // else { | ||||||
|             // Fallback implementation |             // Fallback implementation | ||||||
|             GameObject gameObj = new(core.name); |             GameObject gameObj = new(core.name); | ||||||
|             DifferentialDrive component = gameObj.AddComponent<DifferentialDrive>(); |             DifferentialDrive component = gameObj.AddComponent<DifferentialDrive>(); | ||||||
| @ -33,39 +32,46 @@ namespace RoboidControl.Unity { | |||||||
|             rb.isKinematic = false; |             rb.isKinematic = false; | ||||||
|             rb.mass = 0.5f; |             rb.mass = 0.5f; | ||||||
|             return component; |             return component; | ||||||
|             } |             // } | ||||||
|         } |         } | ||||||
| 
 | 
 | ||||||
|         protected override void HandleBinary() { |         protected override void HandleBinary() { | ||||||
|  |             Debug.Log("Diff drive handle Binary"); | ||||||
|             RoboidControl.DifferentialDrive coreDrive = core as RoboidControl.DifferentialDrive; |             RoboidControl.DifferentialDrive coreDrive = core as RoboidControl.DifferentialDrive; | ||||||
|  |             RoboidControl.Unity.Wheel[] motors = null; | ||||||
|  |             if (coreDrive.wheelRadius <= 0 || coreDrive.wheelSeparation <= 0) | ||||||
|  |                 return; | ||||||
|  | 
 | ||||||
|             if (leftWheel == null) { |             if (leftWheel == null) { | ||||||
|                 GameObject leftWheelObj = new GameObject("Left wheel"); |                 motors = GetComponentsInChildren<Wheel>(); | ||||||
|                 leftWheelObj.transform.SetParent(this.transform); |                 foreach (var motor in motors) { | ||||||
|                 leftWheel = leftWheelObj.AddComponent<WheelCollider>(); |                     if (motor.core.id == coreDrive.leftWheel.id) | ||||||
|                 leftWheel.mass = 0.1f; |                         leftWheel = motor; | ||||||
|                 leftWheel.suspensionDistance = 0.01f; |  | ||||||
|                 leftWheel.suspensionSpring = new JointSpring { |  | ||||||
|                     spring = 100f, // Very high spring value to make it rigid |  | ||||||
|                     damper = 10f,    // Low damping (could be adjusted for slight 'bounciness') |  | ||||||
|                     targetPosition = 0.5f // Neutral position (middle of the suspension travel) |  | ||||||
|                 }; |  | ||||||
|                 } |                 } | ||||||
|             leftWheel.radius = coreDrive.wheelRadius; |                 if (leftWheel == null) | ||||||
|             leftWheel.center = new Vector3(-coreDrive.wheelSeparation / 2, 0, 0); |                     leftWheel = Wheel.Create(this.GetComponent<Rigidbody>(), coreDrive.leftWheel.id); | ||||||
|  |             } | ||||||
|  |             if (leftWheel != null) { | ||||||
|  |                 leftWheel.wheelCollider.radius = coreDrive.wheelRadius; | ||||||
|  |                 leftWheel.wheelCollider.center = new Vector3(-coreDrive.wheelSeparation / 2, 0, 0); | ||||||
|  |                 leftWheel.transform.position = Vector3.zero; // position is done with the center, but only X direction is supported right now... | ||||||
|  |             } | ||||||
|  | 
 | ||||||
|             if (rightWheel == null) { |             if (rightWheel == null) { | ||||||
|                 GameObject rightWheelObj = new GameObject("Right wheel"); |                 if (motors == null) | ||||||
|                 rightWheelObj.transform.SetParent(this.transform); |                     motors = GetComponentsInChildren<Wheel>(); | ||||||
|                 rightWheel = rightWheelObj.AddComponent<WheelCollider>(); |                 foreach (var motor in motors) { | ||||||
|                 rightWheel.mass = 0.1f; |                     if (motor.core.id == coreDrive.rightWheel.id) | ||||||
|                 rightWheel.suspensionDistance = 0.01f; |                         rightWheel = motor; | ||||||
|                 rightWheel.suspensionSpring = new JointSpring { |                 } | ||||||
|                     spring = 100f, // Very high spring value to make it rigid |                 if (rightWheel == null) | ||||||
|                     damper = 10f,    // Low damping (could be adjusted for slight 'bounciness') |                     rightWheel = Wheel.Create(this.GetComponent<Rigidbody>(), coreDrive.rightWheel.id); | ||||||
|                     targetPosition = 0.5f // Neutral position (middle of the suspension travel) |             } | ||||||
|                 }; |             if (rightWheel != null && coreDrive.wheelRadius > 0 && coreDrive.wheelSeparation > 0) { | ||||||
|  |                 rightWheel.wheelCollider.radius = coreDrive.wheelRadius; | ||||||
|  |                 rightWheel.wheelCollider.center = new Vector3(coreDrive.wheelSeparation / 2, 0, 0); | ||||||
|  |                 rightWheel.transform.position = Vector3.zero; // position is done with the center, but only X direction is supported right now... | ||||||
|             } |             } | ||||||
|             rightWheel.radius = coreDrive.wheelRadius; |  | ||||||
|             rightWheel.center = new Vector3(coreDrive.wheelSeparation / 2, 0, 0); |  | ||||||
|         } |         } | ||||||
|     } |     } | ||||||
| 
 | 
 | ||||||
|  | |||||||
							
								
								
									
										49
									
								
								Unity/Motor.cs
									
									
									
									
									
										Normal file
									
								
							
							
						
						
									
										49
									
								
								Unity/Motor.cs
									
									
									
									
									
										Normal file
									
								
							| @ -0,0 +1,49 @@ | |||||||
|  | #if UNITY_5_3_OR_NEWER | ||||||
|  | using UnityEngine; | ||||||
|  | 
 | ||||||
|  | namespace RoboidControl.Unity { | ||||||
|  |     public class Motor : Thing { | ||||||
|  |         /// <summary> | ||||||
|  |         /// Create the Unity representation | ||||||
|  |         /// </summary> | ||||||
|  |         /// <param name="core">The core motor</param> | ||||||
|  |         /// <returns>The Unity representation of a motor</returns> | ||||||
|  |         public static Motor Create(RoboidControl.Motor core) { | ||||||
|  |             GameObject prefab = (GameObject)Resources.Load("Motor"); | ||||||
|  |             if (prefab != null) { | ||||||
|  |                 // Use resource prefab when available | ||||||
|  |                 GameObject gameObj = Instantiate(prefab); | ||||||
|  |                 Motor component = gameObj.GetComponent<Motor>(); | ||||||
|  |                 if (component != null) | ||||||
|  |                     component.core = core; | ||||||
|  |                 return component; | ||||||
|  |             } | ||||||
|  |             else { | ||||||
|  |                 // Fallback implementation | ||||||
|  |                 GameObject gameObj = new(core.name); | ||||||
|  |                 Motor component = gameObj.AddComponent<Motor>(); | ||||||
|  |                 component.Init(core); | ||||||
|  | 
 | ||||||
|  |                 Rigidbody rb = gameObj.AddComponent<Rigidbody>(); | ||||||
|  |                 rb.isKinematic = true; | ||||||
|  |                 return component; | ||||||
|  |             } | ||||||
|  |         } | ||||||
|  | 
 | ||||||
|  |         public float rotationSpeed = 0.0f; | ||||||
|  | 
 | ||||||
|  |         protected override void HandleBinary() { | ||||||
|  |             RoboidControl.Motor coreMotor = core as RoboidControl.Motor; | ||||||
|  |             this.rotationSpeed = coreMotor.targetSpeed; | ||||||
|  |         } | ||||||
|  | 
 | ||||||
|  |         protected override void Update() { | ||||||
|  |             base.Update(); | ||||||
|  |             // We rotate the first child of the motor, which should be the axle. | ||||||
|  |             if (this.transform.childCount > 0) { | ||||||
|  |                 this.transform.GetChild(0).Rotate(360 * this.rotationSpeed, 0, 0); | ||||||
|  |             } | ||||||
|  |         } | ||||||
|  |     } | ||||||
|  | } | ||||||
|  | #endif | ||||||
| @ -34,6 +34,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: | ||||||
|  |                 //     //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: |                 case RoboidControl.Thing coreThing: | ||||||
|                     if (coreThing.component != null) { |                     if (coreThing.component != null) { | ||||||
|                         Thing thing = Thing.Create(coreThing); |                         Thing thing = Thing.Create(coreThing); | ||||||
|  | |||||||
| @ -7,14 +7,15 @@ namespace RoboidControl.Unity { | |||||||
| 
 | 
 | ||||||
|     public class SiteServer : Participant { |     public class SiteServer : Participant { | ||||||
| 
 | 
 | ||||||
|         public RoboidControl.SiteServer site; |         //public RoboidControl.SiteServer site; | ||||||
|  |         public RoboidControl.SiteServer site => this.coreParticipant as RoboidControl.SiteServer; | ||||||
| 
 | 
 | ||||||
|         public Queue<RoboidControl.Thing> thingQueue = new(); |         //public Queue<RoboidControl.Thing> thingQueue = new(); | ||||||
| 
 | 
 | ||||||
|         protected virtual void Awake() { |         protected virtual void Awake() { | ||||||
|             Console.SetOut(new UnityLogWriter()); |             Console.SetOut(new UnityLogWriter()); | ||||||
| 
 | 
 | ||||||
|             site = new RoboidControl.SiteServer(port); |             this.coreParticipant = new RoboidControl.SiteServer(port); | ||||||
|         } |         } | ||||||
| 
 | 
 | ||||||
|         void OnApplicationQuit() { |         void OnApplicationQuit() { | ||||||
| @ -26,12 +27,12 @@ namespace RoboidControl.Unity { | |||||||
|             if (site == null) |             if (site == null) | ||||||
|                 return; |                 return; | ||||||
| 
 | 
 | ||||||
|             if (site.updateQueue.TryDequeue(out RoboidControl.Participant.UpdateEvent e)) |             while (site.updateQueue.TryDequeue(out RoboidControl.Participant.UpdateEvent e)) | ||||||
|                 HandleUpdateEvent(e); |                 HandleUpdateEvent(e); | ||||||
| 
 | 
 | ||||||
|             site.Update((ulong)(Time.time * 1000)); |             site.Update((ulong)(Time.time * 1000)); | ||||||
|             while (thingQueue.TryDequeue(out RoboidControl.Thing thing)) |             // while (thingQueue.TryDequeue(out RoboidControl.Thing thing)) | ||||||
|                 thing.CreateComponent(); |             //     thing.CreateComponent(); | ||||||
|         } |         } | ||||||
| 
 | 
 | ||||||
|         private void HandleUpdateEvent(RoboidControl.Participant.UpdateEvent e) { |         private void HandleUpdateEvent(RoboidControl.Participant.UpdateEvent e) { | ||||||
| @ -43,7 +44,6 @@ namespace RoboidControl.Unity { | |||||||
|                     break; |                     break; | ||||||
|                 case ThingMsg.id: |                 case ThingMsg.id: | ||||||
|                     HandleThingEvent(e); |                     HandleThingEvent(e); | ||||||
|                     //e.thing.CreateComponent(); |  | ||||||
|                     break; |                     break; | ||||||
|             } |             } | ||||||
|         } |         } | ||||||
|  | |||||||
| @ -47,10 +47,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.participant = FindAnyObjectByType<SiteServer>(); |             this.participant = FindAnyObjectByType<SiteServer>(); | ||||||
|             core.owner = this.participant.coreParticipant; |             core.owner = this.participant.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); | ||||||
|  |                 this.transform.localPosition = Vector3.zero; | ||||||
|  |             } | ||||||
| 
 | 
 | ||||||
|             if (core.position != null) |             if (core.position != null) | ||||||
|                 this.transform.localPosition = core.position.ToVector3(); |                 this.transform.localPosition = core.position.ToVector3(); | ||||||
| @ -62,14 +65,7 @@ namespace RoboidControl.Unity { | |||||||
|         /// Update the Unity representation |         /// Update the Unity representation | ||||||
|         /// </summary> |         /// </summary> | ||||||
|         protected virtual void Update() { |         protected virtual void Update() { | ||||||
|             if (core == null) { |             UpdateThing(); | ||||||
|                 // Debug.Log("Core thing is gone, self destruct in 0 seconds..."); |  | ||||||
|                 Destroy(this); |  | ||||||
|                 return; |  | ||||||
|             } |  | ||||||
| 
 |  | ||||||
|             if (core.updateQueue.TryDequeue(out RoboidControl.Thing.UpdateEvent e)) |  | ||||||
|                 HandleUpdateEvent(e); |  | ||||||
| 
 | 
 | ||||||
|             if (core.linearVelocity != null && core.linearVelocity.distance != 0) { |             if (core.linearVelocity != null && core.linearVelocity.distance != 0) { | ||||||
|                 Vector3 direction = Quaternion.AngleAxis(core.linearVelocity.direction.horizontal, Vector3.up) * Vector3.forward; |                 Vector3 direction = Quaternion.AngleAxis(core.linearVelocity.direction.horizontal, Vector3.up) * Vector3.forward; | ||||||
| @ -82,6 +78,17 @@ namespace RoboidControl.Unity { | |||||||
|             } |             } | ||||||
|         } |         } | ||||||
| 
 | 
 | ||||||
|  |         public void UpdateThing() { | ||||||
|  |             if (core == null) { | ||||||
|  |                 // Debug.Log("Core thing is gone, self destruct in 0 seconds..."); | ||||||
|  |                 Destroy(this); | ||||||
|  |                 return; | ||||||
|  |             } | ||||||
|  | 
 | ||||||
|  |             if (core.updateQueue.TryDequeue(out RoboidControl.Thing.UpdateEvent e)) | ||||||
|  |                 HandleUpdateEvent(e); | ||||||
|  |         } | ||||||
|  | 
 | ||||||
|         private void HandleUpdateEvent(RoboidControl.Thing.UpdateEvent e) { |         private void HandleUpdateEvent(RoboidControl.Thing.UpdateEvent e) { | ||||||
|             switch (e.messageId) { |             switch (e.messageId) { | ||||||
|                 case ThingMsg.id: |                 case ThingMsg.id: | ||||||
| @ -101,10 +108,7 @@ namespace RoboidControl.Unity { | |||||||
|                     this.modelUrl = core.modelUrl; |                     this.modelUrl = core.modelUrl; | ||||||
|                     break; |                     break; | ||||||
|                 case PoseMsg.Id: |                 case PoseMsg.Id: | ||||||
|                     if (core.linearVelocity.distance == 0) |                     this.HandlePose(); | ||||||
|                         this.transform.localPosition = core.position.ToVector3(); |  | ||||||
|                     if (core.angularVelocity.distance == 0) |  | ||||||
|                         this.transform.localRotation = core.orientation.ToQuaternion(); |  | ||||||
|                     break; |                     break; | ||||||
|                 case BinaryMsg.Id: |                 case BinaryMsg.Id: | ||||||
|                     this.HandleBinary(); |                     this.HandleBinary(); | ||||||
| @ -112,14 +116,6 @@ namespace RoboidControl.Unity { | |||||||
|             } |             } | ||||||
|         } |         } | ||||||
| 
 | 
 | ||||||
|         private void PoseChanged() { |  | ||||||
|             //Debug.Log($"{this} pose changed"); |  | ||||||
|             if (core.positionUpdated) |  | ||||||
|                 this.transform.localPosition = core.position.ToVector3(); |  | ||||||
|             if (core.orientationUpdated) |  | ||||||
|                 this.transform.localRotation = core.orientation.ToQuaternion(); |  | ||||||
|         } |  | ||||||
| 
 |  | ||||||
|         private IEnumerator LoadJPG() { |         private IEnumerator LoadJPG() { | ||||||
|             UnityWebRequest request = UnityWebRequestTexture.GetTexture(core.modelUrl); |             UnityWebRequest request = UnityWebRequestTexture.GetTexture(core.modelUrl); | ||||||
|             yield return request.SendWebRequest(); |             yield return request.SendWebRequest(); | ||||||
| @ -147,6 +143,13 @@ namespace RoboidControl.Unity { | |||||||
|             } |             } | ||||||
|         } |         } | ||||||
| 
 | 
 | ||||||
|  |         protected virtual void HandlePose() { | ||||||
|  |             if (core.linearVelocity.distance == 0) | ||||||
|  |                 this.transform.localPosition = core.position.ToVector3(); | ||||||
|  |             if (core.angularVelocity.distance == 0) | ||||||
|  |                 this.transform.localRotation = core.orientation.ToQuaternion(); | ||||||
|  | 
 | ||||||
|  |         } | ||||||
|         protected virtual void HandleBinary() { } |         protected virtual void HandleBinary() { } | ||||||
| 
 | 
 | ||||||
|     } |     } | ||||||
|  | |||||||
| @ -13,7 +13,7 @@ namespace RoboidControl.Unity { | |||||||
| 		/// The core touch sensor | 		/// The core touch sensor | ||||||
| 		/// </summary> | 		/// </summary> | ||||||
| 		public RoboidControl.TouchSensor coreSensor { | 		public RoboidControl.TouchSensor coreSensor { | ||||||
| 			get => (RoboidControl.TouchSensor)base.core; | 			get => base.core as RoboidControl.TouchSensor; | ||||||
| 			set => base.core = value; | 			set => base.core = value; | ||||||
| 		} | 		} | ||||||
| 
 | 
 | ||||||
| @ -74,6 +74,7 @@ namespace RoboidControl.Unity { | |||||||
| 
 | 
 | ||||||
| 			Debug.Log($"*** {this} Touch"); | 			Debug.Log($"*** {this} Touch"); | ||||||
| 			this.coreSensor.touchedSomething = true;		 | 			this.coreSensor.touchedSomething = true;		 | ||||||
|  | 			this.core.updateQueue.Enqueue(new RoboidControl.Thing.UpdateEvent(BinaryMsg.Id)); | ||||||
| 		} | 		} | ||||||
| 		private void OnTriggerExit(Collider other) { | 		private void OnTriggerExit(Collider other) { | ||||||
| 			if (other.isTrigger) | 			if (other.isTrigger) | ||||||
|  | |||||||
							
								
								
									
										96
									
								
								Unity/Wheel.cs
									
									
									
									
									
										Normal file
									
								
							
							
						
						
									
										96
									
								
								Unity/Wheel.cs
									
									
									
									
									
										Normal file
									
								
							| @ -0,0 +1,96 @@ | |||||||
|  | #if UNITY_5_3_OR_NEWER | ||||||
|  | using UnityEngine; | ||||||
|  | 
 | ||||||
|  | namespace RoboidControl.Unity { | ||||||
|  |     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 Wheel Create(RoboidControl.Motor core, float wheelRadius) { | ||||||
|  |             GameObject prefab = (GameObject)Resources.Load("Wheel"); | ||||||
|  |             if (prefab != null) { | ||||||
|  |                 // Use resource prefab when available | ||||||
|  |                 GameObject gameObj = Instantiate(prefab); | ||||||
|  |                 Wheel component = gameObj.GetComponent<Wheel>(); | ||||||
|  |                 if (component != null) | ||||||
|  |                     component.core = core; | ||||||
|  |                 return component; | ||||||
|  |             } | ||||||
|  |             else { | ||||||
|  |                 // Fallback implementation | ||||||
|  |                 GameObject gameObj = new(core.name); | ||||||
|  |                 Wheel component = gameObj.AddComponent<Wheel>(); | ||||||
|  |                 component.Init(core); | ||||||
|  |                 component.wheelCollider = gameObj.AddComponent<WheelCollider>(); | ||||||
|  |                 component.wheelCollider.mass = 0.1f; | ||||||
|  |                 component.wheelCollider.suspensionDistance = 0.01f; | ||||||
|  |                 component.wheelCollider.suspensionSpring = new JointSpring { | ||||||
|  |                     spring = 100f, // Very high spring value to make it rigid | ||||||
|  |                     damper = 10f,    // Low damping (could be adjusted for slight 'bounciness') | ||||||
|  |                     targetPosition = 0.5f // Neutral position (middle of the suspension travel) | ||||||
|  |                 }; | ||||||
|  |                 Debug.Log("Create " + core.name); | ||||||
|  |                 return component; | ||||||
|  |             } | ||||||
|  |         } | ||||||
|  |         public static Wheel Create(Rigidbody rb, byte thingId) { | ||||||
|  |             GameObject prefab = (GameObject)Resources.Load("Wheel"); | ||||||
|  |             if (prefab != null) { | ||||||
|  |                 // Use resource prefab when available | ||||||
|  |                 GameObject gameObj = Instantiate(prefab); | ||||||
|  |                 Wheel component = gameObj.GetComponent<Wheel>(); | ||||||
|  |                 if (component != null) | ||||||
|  |                     component.core = new RoboidControl.Thing(RoboidControl.Thing.Type.UncontrolledMotor, false); | ||||||
|  |                 return component; | ||||||
|  |             } | ||||||
|  |             else { | ||||||
|  |                 // Fallback implementation | ||||||
|  |                 GameObject gameObj = new("Wheel"); | ||||||
|  |                 gameObj.transform.parent = rb.transform; | ||||||
|  |                 Wheel component = gameObj.AddComponent<Wheel>(); | ||||||
|  |                 SiteServer participant = FindAnyObjectByType<SiteServer>(); | ||||||
|  |                 RoboidControl.Thing core = participant.coreParticipant.Get(thingId); | ||||||
|  |                 if (core == null) | ||||||
|  |                     core = new(participant.coreParticipant, RoboidControl.Thing.Type.UncontrolledMotor, thingId, false); | ||||||
|  |                 else { | ||||||
|  | ; | ||||||
|  |                 } | ||||||
|  |                 component.Init(core); | ||||||
|  |                 component.wheelCollider = gameObj.AddComponent<WheelCollider>(); | ||||||
|  |                 component.wheelCollider.mass = 0.1f; | ||||||
|  |                 component.wheelCollider.suspensionDistance = 0.01f; | ||||||
|  |                 component.wheelCollider.suspensionSpring = new JointSpring { | ||||||
|  |                     spring = 100f, // Very high spring value to make it rigid | ||||||
|  |                     damper = 10f,    // Low damping (could be adjusted for slight 'bounciness') | ||||||
|  |                     targetPosition = 0.5f // Neutral position (middle of the suspension travel) | ||||||
|  |                 }; | ||||||
|  |                 Debug.Log("Create placeholder Wheel "); | ||||||
|  |                 return component; | ||||||
|  |             } | ||||||
|  |         } | ||||||
|  | 
 | ||||||
|  |         public WheelCollider wheelCollider; | ||||||
|  | 
 | ||||||
|  |         protected override void HandlePose() { | ||||||
|  |             this.wheelCollider.center = core.position.ToVector3(); | ||||||
|  |             this.transform.position = Vector3.zero; // position is done with the center | ||||||
|  |         } | ||||||
|  | 
 | ||||||
|  |         protected override void Update() { | ||||||
|  |             UpdateThing(); | ||||||
|  | 
 | ||||||
|  |             if (wheelCollider.radius > 0) { | ||||||
|  |                 float targetRotationSpeed = this.rotationSpeed * 2 * Mathf.PI; // 1 rotation per second in radians | ||||||
|  | 
 | ||||||
|  |                 // Calculate the required motor torque | ||||||
|  |                 float requiredTorque = (targetRotationSpeed * wheelCollider.mass) / wheelCollider.radius; | ||||||
|  | 
 | ||||||
|  |                 // Set the motor torque | ||||||
|  |                 wheelCollider.motorTorque = requiredTorque; | ||||||
|  |             } | ||||||
|  |         } | ||||||
|  |     } | ||||||
|  | } | ||||||
|  | #endif | ||||||
| @ -35,8 +35,19 @@ namespace RoboidControl { | |||||||
|         /// <summary> |         /// <summary> | ||||||
|         /// Create an empty message for sending |         /// Create an empty message for sending | ||||||
|         /// </summary> |         /// </summary> | ||||||
|         /// <param name="networkId">The netowork ID of the thing</param> |         /// <param name="thingId">The thing sending the binary message</param> | ||||||
|         /// <param name="thingId">The ID of the thing</param> |         public BinaryMsg(Thing thing) : base() { | ||||||
|  |             this.networkId = thing.owner.networkId; | ||||||
|  |             this.thingId = thing.id; | ||||||
|  |             this.thing = thing; | ||||||
|  |             this.data = this.thing.GenerateBinary(); | ||||||
|  |             this.dataLength = (byte)this.data.Length; | ||||||
|  |         } | ||||||
|  |         /// <summary> | ||||||
|  |         /// Create an empty message for sending | ||||||
|  |         /// </summary> | ||||||
|  |         /// <param name="networkId">The network ID of the thing</param> | ||||||
|  |         /// <param name="thingId">The thing sending the binary message</param> | ||||||
|         public BinaryMsg(byte networkId, Thing thing) : base() { |         public BinaryMsg(byte networkId, Thing thing) : base() { | ||||||
|             this.networkId = networkId; |             this.networkId = networkId; | ||||||
|             this.thingId = thing.id; |             this.thingId = thing.id; | ||||||
|  | |||||||
| @ -18,10 +18,10 @@ namespace RoboidControl { | |||||||
|         /// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</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> |         /// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param> | ||||||
|         public DifferentialDrive(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.DifferentialDrive, thingId, invokeEvent) { |         public DifferentialDrive(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.DifferentialDrive, thingId, invokeEvent) { | ||||||
|             Thing leftWheel = new(this) { |             Motor leftWheel = new(this) { | ||||||
|                 name = "Left Wheel" |                 name = "Left Wheel" | ||||||
|             }; |             }; | ||||||
|             Thing rightWheel = new(this) { |             Motor rightWheel = new(this) { | ||||||
|                 name = "Right Wheel" |                 name = "Right Wheel" | ||||||
|             }; |             }; | ||||||
|             SetMotors(leftWheel, rightWheel); |             SetMotors(leftWheel, rightWheel); | ||||||
| @ -46,8 +46,8 @@ namespace RoboidControl { | |||||||
|         /// linear and angular velocity. |         /// linear and angular velocity. | ||||||
|         /// @sa SetLinearVelocity SetAngularVelocity |         /// @sa SetLinearVelocity SetAngularVelocity | ||||||
|         public void SetDriveDimensions(float wheelDiameter, float wheelSeparation) { |         public void SetDriveDimensions(float wheelDiameter, float wheelSeparation) { | ||||||
|             this.wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2; |             this._wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2; | ||||||
|             this.wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation; |             this._wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation; | ||||||
|             this.rpsToMs = wheelDiameter * Angle.pi; |             this.rpsToMs = wheelDiameter * Angle.pi; | ||||||
| 
 | 
 | ||||||
|             float distance = this.wheelSeparation / 2; |             float distance = this.wheelSeparation / 2; | ||||||
| @ -60,7 +60,7 @@ namespace RoboidControl { | |||||||
|         /// @brief Congures the motors for the wheels |         /// @brief Congures the motors for the wheels | ||||||
|         /// @param leftWheel The motor for the left wheel |         /// @param leftWheel The motor for the left wheel | ||||||
|         /// @param rightWheel The motor for the right wheel |         /// @param rightWheel The motor for the right wheel | ||||||
|         public void SetMotors(Thing leftWheel, Thing rightWheel) { |         public void SetMotors(Motor leftWheel, Motor rightWheel) { | ||||||
|             float distance = this.wheelSeparation / 2; |             float distance = this.wheelSeparation / 2; | ||||||
| 
 | 
 | ||||||
|             this.leftWheel = leftWheel; |             this.leftWheel = leftWheel; | ||||||
| @ -78,10 +78,14 @@ namespace RoboidControl { | |||||||
|         /// @param speedRight The speed of the right wheel in degrees per second. |         /// @param speedRight The speed of the right wheel in degrees per second. | ||||||
|         /// Positive moves the robot in the forward direction. |         /// Positive moves the robot in the forward direction. | ||||||
|         public void SetWheelVelocity(float speedLeft, float speedRight) { |         public void SetWheelVelocity(float speedLeft, float speedRight) { | ||||||
|             if (this.leftWheel != null) |             if (this.leftWheel != null) { | ||||||
|                 this.leftWheel.angularVelocity = new Spherical(speedLeft, Direction.left); |                 this.leftWheel.targetSpeed = speedLeft; | ||||||
|             if (this.rightWheel != null) |                 //this.leftWheel.angularVelocity = new Spherical(speedLeft, Direction.left); | ||||||
|                 this.rightWheel.angularVelocity = new Spherical(speedRight, Direction.right); |             } | ||||||
|  |             if (this.rightWheel != null) { | ||||||
|  |                 this.rightWheel.targetSpeed = speedRight; | ||||||
|  |                 //this.rightWheel.angularVelocity = new Spherical(speedRight, Direction.right); | ||||||
|  |             } | ||||||
|         } |         } | ||||||
| 
 | 
 | ||||||
|         /// @copydoc RoboidControl::Thing::Update(unsigned long) |         /// @copydoc RoboidControl::Thing::Update(unsigned long) | ||||||
| @ -104,17 +108,19 @@ namespace RoboidControl { | |||||||
|         } |         } | ||||||
| 
 | 
 | ||||||
|         /// @brief The radius of a wheel in meters |         /// @brief The radius of a wheel in meters | ||||||
|         public float wheelRadius = 1.0f; |         private float _wheelRadius = 0.0f; | ||||||
|  |         public float wheelRadius { get => _wheelRadius; } | ||||||
|         /// @brief The distance between the wheels in meters |         /// @brief The distance between the wheels in meters | ||||||
|         public float wheelSeparation = 1.0f; |         private float _wheelSeparation = 0.0f; | ||||||
|  |         public float wheelSeparation { get => _wheelSeparation; } | ||||||
| 
 | 
 | ||||||
|         /// @brief Convert revolutions per second to meters per second |         /// @brief Convert revolutions per second to meters per second | ||||||
|         protected float rpsToMs = 1.0f; |         protected float rpsToMs = 1.0f; | ||||||
| 
 | 
 | ||||||
|         /// @brief The left wheel |         /// @brief The left wheel | ||||||
|         public Thing leftWheel = null; |         public Motor leftWheel = null; | ||||||
|         /// @brief The right wheel |         /// @brief The right wheel | ||||||
|         public Thing rightWheel = null; |         public Motor rightWheel = null; | ||||||
| 
 | 
 | ||||||
|         bool sendBinary = false; |         bool sendBinary = false; | ||||||
|         public override byte[] GenerateBinary() { |         public override byte[] GenerateBinary() { | ||||||
| @ -134,11 +140,11 @@ 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); |             this.leftWheel = this.owner.Get(leftWheelId) as Motor; | ||||||
|             byte rightWheelId = data[ix++]; |             byte rightWheelId = data[ix++]; | ||||||
|             this.rightWheel = this.owner.Get(rightWheelId); |             this.rightWheel = this.owner.Get(rightWheelId) as Motor; | ||||||
|             this.wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix); |             this._wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix); | ||||||
|             this.wheelSeparation = LowLevelMessages.ReceiveFloat16(data, ref ix); |             this._wheelSeparation = LowLevelMessages.ReceiveFloat16(data, ref ix); | ||||||
|             this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id)); |             this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id)); | ||||||
|         } |         } | ||||||
|     }; |     }; | ||||||
|  | |||||||
							
								
								
									
										45
									
								
								src/Things/Motor.cs
									
									
									
									
									
										Normal file
									
								
							
							
						
						
									
										45
									
								
								src/Things/Motor.cs
									
									
									
									
									
										Normal file
									
								
							| @ -0,0 +1,45 @@ | |||||||
|  | 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) { } | ||||||
|  | 
 | ||||||
|  |         /// @brief Motor turning direction | ||||||
|  |         public enum Direction { | ||||||
|  |             Clockwise = 1, | ||||||
|  |             CounterClockwise = -1 | ||||||
|  |         }; | ||||||
|  |         /// @brief The forward turning direction of the motor | ||||||
|  |         public Direction direction = Direction.Clockwise; | ||||||
|  | 
 | ||||||
|  |         protected float currentTargetSpeed = 0; | ||||||
|  | 
 | ||||||
|  |         private float _targetSpeed; | ||||||
|  |         /// <summary> | ||||||
|  |         /// The speed between -1 (full reverse), 0 (stop) and 1 (full forward) | ||||||
|  |         /// </summary> | ||||||
|  |         public float targetSpeed { | ||||||
|  |             get => _targetSpeed; | ||||||
|  |             set { | ||||||
|  |                 if (value != _targetSpeed) { | ||||||
|  |                     _targetSpeed = Float.Clamp(value, -1, 1); | ||||||
|  |                     updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id)); | ||||||
|  |                     owner.Send(new BinaryMsg(this)); | ||||||
|  |                 } | ||||||
|  |             } | ||||||
|  |         } | ||||||
|  | 
 | ||||||
|  |         public override byte[] GenerateBinary() { | ||||||
|  |             byte[] data = new byte[1]; | ||||||
|  |             byte ix = 0; | ||||||
|  |             data[ix++] = (byte)(this.targetSpeed * 127); | ||||||
|  |             return data; | ||||||
|  |         } | ||||||
|  |         public override void ProcessBinary(byte[] data) { | ||||||
|  |             this.targetSpeed = (float)data[0] / 127; | ||||||
|  |         } | ||||||
|  |     } | ||||||
|  | 
 | ||||||
|  | } | ||||||
		Loading…
	
	
			
			x
			
			
		
	
		Reference in New Issue
	
	Block a user