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

First step to adding differential dirve

Browse Source
This commit is contained in:
Pascal Serrarens 2025-05-03 13:16:57 +02:00
parent 574a8c742b
commit 25edc506a0
10 changed files with 212 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
Examples/BB2B/BB2B.cs
View File

@ -1,39 +1,40 @@

namespace RoboidControl {
public class BB2B : Thing {
readonly DifferentialDrive drive;
readonly TouchSensor touchLeft;
readonly TouchSensor touchRight;
// The robot is based on a differential drive
public class BB2B : DifferentialDrive {
readonly DifferentialDrive drive;
readonly TouchSensor touchLeft;
readonly TouchSensor touchRight;
public BB2B() : base(128) { // thingType = 128
public BB2B(Participant owner) : base(owner) {
this.name = "BB2B";
this.wheelRadius = 0.032f;
this.wheelSeparation = 0.128f;
// The robot's propulsion is a differential drive
drive = new();
// Is has a touch sensor at the front left of the roboid
touchLeft = new(this);
// and other one on the right
touchRight = new(this);
}
// Is has a touch sensor at the front left of the roboid
touchLeft = new(drive);
// and other one on the right
touchRight = new(drive);
public override void Update(ulong currentTimeMs, bool recurse = true) {
// 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;
// The right wheel does the same, but instead is controlled by
// touches on the left side
float rightWheelSpeed = touchLeft.touchedSomething ? -600.0f : 600.0f;
// When both sides are touching something, both wheels will turn backward
// and the roboid will move backwards
this.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
base.Update(currentTimeMs, recurse);
}
}
public override void Update(ulong currentTimeMs, bool recurse = true) {
// 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;
// The right wheel does the same, but instead is controlled by
// touches on the left side
float rightWheelSpeed = touchLeft.touchedSomething ? -600.0f : 600.0f;
// When both sides are touching something, both wheels will turn backward
// and the roboid will move backwards
drive.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
base.Update(currentTimeMs, recurse);
}
}
}

2
Examples/BB2B/Program.cs
View File

@ -3,7 +3,7 @@ using RoboidControl;
class Program {
static void Main() {
BB2B bb2b = new();
BB2B bb2b = new(ParticipantUDP.Isolated());
while (true) {
bb2b.Update();

108
Unity/DifferentialDrive.cs Normal file
View File

@ -0,0 +1,108 @@
#if UNITY_5_3_OR_NEWER
using System.Linq;
using UnityEngine;
namespace RoboidControl.Unity {
public class DifferentialDrive : Thing {
public WheelCollider leftWheel;
public WheelCollider rightWheel;
/// <summary>
/// Create the Unity representation
/// </summary>
/// <param name="core">The core touch sensor</param>
/// <returns>The Unity representation of the touch sensor</returns>
public static DifferentialDrive Create(RoboidControl.DifferentialDrive core) {
GameObject gameObj = new(core.name);
DifferentialDrive component = gameObj.AddComponent<DifferentialDrive>();
component.Init(core);
Rigidbody rb = gameObj.AddComponent<Rigidbody>();
rb.isKinematic = false;
rb.mass = 0.5f;
return component;
}
protected override void HandleBinary() {
RoboidControl.DifferentialDrive drive = core as RoboidControl.DifferentialDrive;
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 = 1000f, // Very high spring value to make it rigid
damper = 100f, // Low damping (could be adjusted for slight 'bounciness')
targetPosition = 0.5f // Neutral position (middle of the suspension travel)
};
leftWheel.radius = drive.wheelRadius;
leftWheel.center = new Vector3(-drive.wheelSeparation / 2, 0, 0);
}
if (rightWheel == null) {
GameObject rightWheelObj = new GameObject("Left wheel");
rightWheelObj.transform.SetParent(this.transform);
rightWheel = rightWheelObj.AddComponent<WheelCollider>();
rightWheel.mass = 0.1f;
rightWheel.suspensionDistance = 0.01f;
rightWheel.suspensionSpring = new JointSpring {
spring = 1000f, // Very high spring value to make it rigid
damper = 100f, // Low damping (could be adjusted for slight 'bounciness')
targetPosition = 0.5f // Neutral position (middle of the suspension travel)
};
rightWheel.radius = drive.wheelRadius;
rightWheel.center = new Vector3(drive.wheelSeparation / 2, 0, 0);
}
// Thing[] children = this.GetComponentsInChildren<Thing>();
// if (leftWheel == null) {
// leftWheel = children.FirstOrDefault(child => child.core.id == drive.leftWheel.id);
// if (leftWheel == null) {
// RoboidControl.Thing coreThing = new(drive.owner, 0, drive.leftWheel.id, false) {
// name = "Left Wheel"
// };
// leftWheel = Thing.Create(coreThing);
// leftWheel.transform.SetParent(this.transform);
// }
// WheelCollider wheel = this.GetComponent<WheelCollider>();
// if (wheel == null)
// wheel = this.gameObject.AddComponent<WheelCollider>();
// wheel.mass = 0.1f;
// wheel.suspensionDistance = 0.01f;
// wheel.suspensionSpring = new JointSpring {
// spring = 1000f, // Very high spring value to make it rigid
// damper = 100f, // Low damping (could be adjusted for slight 'bounciness')
// targetPosition = 0.5f // Neutral position (middle of the suspension travel)
// };
// wheel.radius = drive.wheelRadius;
// wheel.center = new Vector3(-drive.wheelSeparation / 2, 0, 0);
// }
// if (rightWheel == null) {
// this.rightWheel = children.FirstOrDefault(child => child.core.id == drive.rightWheel.id);
// if (this.rightWheel == null) {
// RoboidControl.Thing coreThing = new(drive.owner, 0, drive.rightWheel.id, false) {
// name = "Right Wheel"
// };
// this.rightWheel = Thing.Create(coreThing);
// this.rightWheel.transform.SetParent(this.transform);
// }
// WheelCollider wheel = this.GetComponent<WheelCollider>();
// if (wheel == null)
// wheel = this.gameObject.AddComponent<WheelCollider>();
// wheel.mass = 0.1f;
// wheel.suspensionDistance = 0.01f;
// wheel.suspensionSpring = new JointSpring {
// spring = 1000f, // Very high spring value to make it rigid
// damper = 100f, // Low damping (could be adjusted for slight 'bounciness')
// targetPosition = 0.5f // Neutral position (middle of the suspension travel)
// };
// wheel.radius = drive.wheelRadius;
// wheel.center = new Vector3(drive.wheelSeparation / 2, 0, 0);
// }
}
}
}
#endif

12
Unity/Participant.cs
View File

@ -24,15 +24,21 @@ namespace RoboidControl.Unity {
}
}
private void HandleThingEvent(RoboidControl.Participant.UpdateEvent e) {
protected virtual void HandleThingEvent(RoboidControl.Participant.UpdateEvent e) {
switch (e.thing) {
case RoboidControl.TouchSensor coreTouchSensor:
TouchSensor touchSensor = TouchSensor.Create(coreTouchSensor);
coreTouchSensor.component = touchSensor;
break;
case RoboidControl.DifferentialDrive coreDrive:
DifferentialDrive differentialDrive = DifferentialDrive.Create(coreDrive);
coreDrive.component = differentialDrive;
break;
case RoboidControl.Thing coreThing:
Thing thing = Thing.Create(coreThing);
coreThing.component = thing;
if (coreThing.component != null) {
Thing thing = Thing.Create(coreThing);
coreThing.component = thing;
}
break;
}
}

3
Unity/SiteServer.cs
View File

@ -42,7 +42,8 @@ namespace RoboidControl.Unity {
participant.coreParticipant = e.participant;
break;
case ThingMsg.id:
e.thing.CreateComponent();
HandleThingEvent(e);
//e.thing.CreateComponent();
break;
}
}

5
Unity/Thing.cs
View File

@ -48,6 +48,7 @@ namespace RoboidControl.Unity {
protected void Init(RoboidControl.Thing core) {
this.core = core;
this.participant = FindAnyObjectByType<SiteServer>();
core.owner = this.participant.coreParticipant;
if (core.parent != null && core.parent.component != null)
this.transform.SetParent(core.parent.component.transform, false);
@ -105,6 +106,9 @@ namespace RoboidControl.Unity {
if (core.angularVelocity.distance == 0)
this.transform.localRotation = core.orientation.ToQuaternion();
break;
case BinaryMsg.Id:
this.HandleBinary();
break;
}
}
@ -143,6 +147,7 @@ namespace RoboidControl.Unity {
}
}
protected virtual void HandleBinary() {}
}

4
Unity/TouchSensor.cs
View File

@ -36,9 +36,7 @@ namespace RoboidControl.Unity {
/// <param name="core">The core touch sensor</param>
/// <returns>The Unity representation of the touch sensor</returns>
public static TouchSensor Create(RoboidControl.TouchSensor core) {
GameObject gameObj = core.name != null ?
new(core.name) :
new("Touch Sensor");
GameObject gameObj = new(core.name);
TouchSensor component = gameObj.AddComponent<TouchSensor>();
component.Init(core);

19
src/Participants/SiteServer.cs
View File

@ -117,16 +117,7 @@ namespace RoboidControl {
Console.WriteLine($"{this.name}: Process thing [{msg.networkId}/{msg.thingId}] {msg.thingType} {msg.parentId} ");
Thing thing = sender.Get(msg.thingId);
if (thing == null) {
switch (msg.thingType) {
case (byte)Thing.Type.TouchSensor:
new TouchSensor(sender, msg.thingId);
break;
}
}
if (thing == null)
thing = new Thing(sender, msg.thingType, msg.thingId);
thing ??= ProcessNewThing(sender, msg);
if (msg.parentId != 0) {
thing.parent = sender.Get(msg.parentId);
@ -139,6 +130,14 @@ namespace RoboidControl {
}
}
protected virtual Thing ProcessNewThing(Participant sender, ThingMsg msg) {
return msg.thingType switch {
Thing.Type.TouchSensor => new TouchSensor(sender, msg.thingId),
Thing.Type.DifferentialDrive => new DifferentialDrive(sender, msg.thingId),
_ => new Thing(sender, msg.thingType, msg.thingId),
};
}
#endregion Receive

48
src/Things/DifferentialDrive.cs
View File

@ -17,7 +17,19 @@ namespace RoboidControl {
/// <param name="owner">The owning participant</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>
public DifferentialDrive(Participant participant, byte thingId = 0, bool invokeEvent = true) : base(participant, Type.DifferentialDrive, thingId, invokeEvent) { }
public DifferentialDrive(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.DifferentialDrive, thingId, invokeEvent) {
Thing leftWheel = new(this) {
name = "Left Wheel"
};
Thing rightWheel = new(this) {
name = "Right Wheel"
};
SetMotors(leftWheel, rightWheel);
sendBinary = true;
owner.Send(new BinaryMsg(owner.networkId, this));
this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id));
}
/// <summary>
/// Create a new child differential drive
/// </summary>
@ -92,17 +104,43 @@ namespace RoboidControl {
}
/// @brief The radius of a wheel in meters
protected float wheelRadius = 1.0f;
public float wheelRadius = 1.0f;
/// @brief The distance between the wheels in meters
protected float wheelSeparation = 1.0f;
public float wheelSeparation = 1.0f;
/// @brief Convert revolutions per second to meters per second
protected float rpsToMs = 1.0f;
/// @brief The left wheel
protected Thing leftWheel = null;
public Thing leftWheel = null;
/// @brief The right wheel
protected Thing rightWheel = null;
public Thing rightWheel = null;
bool sendBinary = false;
public override byte[] GenerateBinary() {
if (!sendBinary)
return System.Array.Empty<byte>();
byte[] data = new byte[6];
byte ix = 0;
data[ix++] = leftWheel.id;
data[ix++] = rightWheel.id;
LowLevelMessages.SendFloat16(data, ref ix, wheelRadius);
LowLevelMessages.SendFloat16(data, ref ix, wheelSeparation);
sendBinary = false;
return data;
}
public override void ProcessBinary(byte[] data) {
byte ix = 0;
byte leftWheelId = data[ix++];
this.leftWheel = this.owner.Get(leftWheelId);
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.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id));
}
};
} // namespace RoboidControl

4
src/Things/TouchSensor.cs
View File

@ -24,7 +24,9 @@ namespace RoboidControl {
/// <param name="parent">The parent thing</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>
public TouchSensor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.TouchSensor, thingId, invokeEvent) { }
public TouchSensor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.TouchSensor, thingId, invokeEvent) {
this.name = "TouchSensor";
}
/// <summary>
/// Value which is true when the sensor is touching something, false otherwise