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

BB2B with Diff.Drive starts to work

Browse Source
This commit is contained in:
Pascal Serrarens 2025-05-08 17:38:38 +02:00
parent 7461ae1595
commit 0a9e11294a
11 changed files with 324 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
Examples/BB2B/BB2B.cs
View File

@ -10,8 +10,9 @@ namespace RoboidControl {
public BB2B(Participant owner) : base(owner) {
this.name = "BB2B";
this.wheelRadius = 0.032f;
this.wheelSeparation = 0.128f;
this.SetDriveDimensions(0.064f, 0.128f);
// this.wheelRadius = 0.032f;
// this.wheelSeparation = 0.128f;
// Is has a touch sensor at the front left of the roboid
touchLeft = new(this) {
@ -28,11 +29,11 @@ namespace RoboidControl {
// The left wheel turns forward when nothing is touched on the right side
// 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
// 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
// and the roboid will move backwards

80
Unity/DifferentialDrive.cs
View File

@ -1,12 +1,11 @@
#if UNITY_5_3_OR_NEWER
using System.Linq;
using UnityEngine;
namespace RoboidControl.Unity {
public class DifferentialDrive : Thing {
public WheelCollider leftWheel;
public WheelCollider rightWheel;
public Wheel leftWheel;
public Wheel rightWheel;
/// <summary>
/// Create the Unity representation
@ -15,15 +14,15 @@ namespace RoboidControl.Unity {
/// <returns>The Unity representation of the touch sensor</returns>
public static DifferentialDrive Create(RoboidControl.DifferentialDrive core) {
GameObject prefab = (GameObject)Resources.Load("DifferentialDrive");
if (prefab != null) {
// Use resource prefab when available
GameObject gameObj = Instantiate(prefab);
DifferentialDrive component = gameObj.GetComponent<DifferentialDrive>();
if (component != null)
component.core = core;
return component;
}
else {
// if (prefab != null) {
// // Use resource prefab when available
// GameObject gameObj = Instantiate(prefab);
// DifferentialDrive component = gameObj.GetComponent<DifferentialDrive>();
// if (component != null)
// component.core = core;
// return component;
// }
// else {
// Fallback implementation
GameObject gameObj = new(core.name);
DifferentialDrive component = gameObj.AddComponent<DifferentialDrive>();
@ -33,39 +32,46 @@ namespace RoboidControl.Unity {
rb.isKinematic = false;
rb.mass = 0.5f;
return component;
}
// }
}
protected override void HandleBinary() {
Debug.Log("Diff drive handle Binary");
RoboidControl.DifferentialDrive coreDrive = core as RoboidControl.DifferentialDrive;
RoboidControl.Unity.Wheel[] motors = null;
if (coreDrive.wheelRadius <= 0 || coreDrive.wheelSeparation <= 0)
return;
if (leftWheel == null) {
GameObject leftWheelObj = new GameObject("Left wheel");
leftWheelObj.transform.SetParent(this.transform);
leftWheel = leftWheelObj.AddComponent<WheelCollider>();
leftWheel.mass = 0.1f;
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)
};
motors = GetComponentsInChildren<Wheel>();
foreach (var motor in motors) {
if (motor.core.id == coreDrive.leftWheel.id)
leftWheel = motor;
}
leftWheel.radius = coreDrive.wheelRadius;
leftWheel.center = new Vector3(-coreDrive.wheelSeparation / 2, 0, 0);
if (leftWheel == null)
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) {
GameObject rightWheelObj = new GameObject("Right wheel");
rightWheelObj.transform.SetParent(this.transform);
rightWheel = rightWheelObj.AddComponent<WheelCollider>();
rightWheel.mass = 0.1f;
rightWheel.suspensionDistance = 0.01f;
rightWheel.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)
};
if (motors == null)
motors = GetComponentsInChildren<Wheel>();
foreach (var motor in motors) {
if (motor.core.id == coreDrive.rightWheel.id)
rightWheel = motor;
}
if (rightWheel == null)
rightWheel = Wheel.Create(this.GetComponent<Rigidbody>(), coreDrive.rightWheel.id);
}
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
View 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

6
Unity/Participant.cs
View File

@ -34,6 +34,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.Thing coreThing:
if (coreThing.component != null) {
Thing thing = Thing.Create(coreThing);

14
Unity/SiteServer.cs
View File

@ -7,14 +7,15 @@ namespace RoboidControl.Unity {
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() {
Console.SetOut(new UnityLogWriter());
site = new RoboidControl.SiteServer(port);
this.coreParticipant = new RoboidControl.SiteServer(port);
}
void OnApplicationQuit() {
@ -26,12 +27,12 @@ namespace RoboidControl.Unity {
if (site == null)
return;
if (site.updateQueue.TryDequeue(out RoboidControl.Participant.UpdateEvent e))
while (site.updateQueue.TryDequeue(out RoboidControl.Participant.UpdateEvent e))
HandleUpdateEvent(e);
site.Update((ulong)(Time.time * 1000));
while (thingQueue.TryDequeue(out RoboidControl.Thing thing))
thing.CreateComponent();
// while (thingQueue.TryDequeue(out RoboidControl.Thing thing))
// thing.CreateComponent();
}
private void HandleUpdateEvent(RoboidControl.Participant.UpdateEvent e) {
@ -43,7 +44,6 @@ namespace RoboidControl.Unity {
break;
case ThingMsg.id:
HandleThingEvent(e);
//e.thing.CreateComponent();
break;
}
}

45
Unity/Thing.cs
View File

@ -47,10 +47,13 @@ namespace RoboidControl.Unity {
protected void Init(RoboidControl.Thing core) {
this.core = core;
this.core.component = this;
this.participant = FindAnyObjectByType<SiteServer>();
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.localPosition = Vector3.zero;
}
if (core.position != null)
this.transform.localPosition = core.position.ToVector3();
@ -62,14 +65,7 @@ namespace RoboidControl.Unity {
/// Update the Unity representation
/// </summary>
protected virtual void Update() {
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);
UpdateThing();
if (core.linearVelocity != null && core.linearVelocity.distance != 0) {
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) {
switch (e.messageId) {
case ThingMsg.id:
@ -101,10 +108,7 @@ namespace RoboidControl.Unity {
this.modelUrl = core.modelUrl;
break;
case PoseMsg.Id:
if (core.linearVelocity.distance == 0)
this.transform.localPosition = core.position.ToVector3();
if (core.angularVelocity.distance == 0)
this.transform.localRotation = core.orientation.ToQuaternion();
this.HandlePose();
break;
case BinaryMsg.Id:
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() {
UnityWebRequest request = UnityWebRequestTexture.GetTexture(core.modelUrl);
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() { }
}

3
Unity/TouchSensor.cs
View File

@ -13,7 +13,7 @@ namespace RoboidControl.Unity {
/// The core touch sensor
/// </summary>
public RoboidControl.TouchSensor coreSensor {
get => (RoboidControl.TouchSensor)base.core;
get => base.core as RoboidControl.TouchSensor;
set => base.core = value;
}
@ -74,6 +74,7 @@ namespace RoboidControl.Unity {
Debug.Log($"*** {this} Touch");
this.coreSensor.touchedSomething = true;
this.core.updateQueue.Enqueue(new RoboidControl.Thing.UpdateEvent(BinaryMsg.Id));
}
private void OnTriggerExit(Collider other) {
if (other.isTrigger)

96
Unity/Wheel.cs Normal file
View 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

15
src/Messages/BinaryMsg.cs
View File

@ -35,8 +35,19 @@ namespace RoboidControl {
/// <summary>
/// Create an empty message for sending
/// </summary>
/// <param name="networkId">The netowork ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="thingId">The thing sending the binary message</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() {
this.networkId = networkId;
this.thingId = thing.id;

40
src/Things/DifferentialDrive.cs
View File

@ -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="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) {
Thing leftWheel = new(this) {
Motor leftWheel = new(this) {
name = "Left Wheel"
};
Thing rightWheel = new(this) {
Motor rightWheel = new(this) {
name = "Right Wheel"
};
SetMotors(leftWheel, rightWheel);
@ -46,8 +46,8 @@ namespace RoboidControl {
/// linear and angular velocity.
/// @sa SetLinearVelocity SetAngularVelocity
public void SetDriveDimensions(float wheelDiameter, float wheelSeparation) {
this.wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
this.wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
this._wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
this._wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
this.rpsToMs = wheelDiameter * Angle.pi;
float distance = this.wheelSeparation / 2;
@ -60,7 +60,7 @@ namespace RoboidControl {
/// @brief Congures the motors for the wheels
/// @param leftWheel The motor for the left 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;
this.leftWheel = leftWheel;
@ -78,10 +78,14 @@ namespace RoboidControl {
/// @param speedRight The speed of the right wheel in degrees per second.
/// Positive moves the robot in the forward direction.
public void SetWheelVelocity(float speedLeft, float speedRight) {
if (this.leftWheel != null)
this.leftWheel.angularVelocity = new Spherical(speedLeft, Direction.left);
if (this.rightWheel != null)
this.rightWheel.angularVelocity = new Spherical(speedRight, Direction.right);
if (this.leftWheel != null) {
this.leftWheel.targetSpeed = speedLeft;
//this.leftWheel.angularVelocity = new Spherical(speedLeft, Direction.left);
}
if (this.rightWheel != null) {
this.rightWheel.targetSpeed = speedRight;
//this.rightWheel.angularVelocity = new Spherical(speedRight, Direction.right);
}
}
/// @copydoc RoboidControl::Thing::Update(unsigned long)
@ -104,17 +108,19 @@ namespace RoboidControl {
}
/// @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
public float wheelSeparation = 1.0f;
private float _wheelSeparation = 0.0f;
public float wheelSeparation { get => _wheelSeparation; }
/// @brief Convert revolutions per second to meters per second
protected float rpsToMs = 1.0f;
/// @brief The left wheel
public Thing leftWheel = null;
public Motor leftWheel = null;
/// @brief The right wheel
public Thing rightWheel = null;
public Motor rightWheel = null;
bool sendBinary = false;
public override byte[] GenerateBinary() {
@ -134,11 +140,11 @@ namespace RoboidControl {
public override void ProcessBinary(byte[] data) {
byte ix = 0;
byte leftWheelId = data[ix++];
this.leftWheel = this.owner.Get(leftWheelId);
this.leftWheel = this.owner.Get(leftWheelId) as Motor;
byte rightWheelId = data[ix++];
this.rightWheel = this.owner.Get(rightWheelId);
this.wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix);
this.wheelSeparation = LowLevelMessages.ReceiveFloat16(data, ref ix);
this.rightWheel = this.owner.Get(rightWheelId) as Motor;
this._wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix);
this._wheelSeparation = LowLevelMessages.ReceiveFloat16(data, ref ix);
this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id));
}
};

45
src/Things/Motor.cs Normal file
View 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;
}
}
}