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

Compare commits

base: RoboidControl:v0.4_dev
Branches Tags
RoboidControl:main
RoboidControl:v0.4_dev
RoboidControl:v0.3.0
..
compare: RoboidControl:main
Branches Tags
RoboidControl:v0.4_dev
RoboidControl:main
RoboidControl:v0.3.0

1 Commits
v0.4_dev ... main

Author SHA1 Message Date
Pascal Serrarens
cf40f311a8 Remove Unity DigitlaSensor 2025-05-01 15:16:19 +02:00
25 changed files with 288 additions and 1976 deletions
Show Stats Expand all files Collapse all files

54
Examples/BB2B/BB2B.cs
View File

@ -1,47 +1,31 @@
using LinearAlgebra; using System.Threading;
using RoboidControl;
namespace RoboidControl { class BB2B {
static void Main() {
// The robot is based on a differential drive // The robot's propulsion is a differential drive
public class BB2B : DifferentialDrive { DifferentialDrive bb2b = new();
readonly DifferentialDrive drive; // Is has a touch sensor at the front left of the roboid
readonly TouchSensor touchLeft; TouchSensor touchLeft = new(bb2b);
readonly TouchSensor touchRight; // and other one on the right
const float speed = 0.5f; TouchSensor touchRight = new(bb2b);
public BB2B(Participant owner) : base(owner) {
this.name = "BB2B";
this.SetMotors(new Motor(this), new Motor(this));
this.SetDriveDimensions(0.064f, 0.128f);
// Is has a touch sensor at the front left of the roboid
touchLeft = new(this) {
position = Spherical.Degrees(0.12f, -30, 0),
orientation = new SwingTwist(-30, 0, 0)
};
touchRight = new(this) {
position = Spherical.Degrees(0.12f, 30, 0),
orientation = new SwingTwist(30, 0, 0)
};
}
public override void Update(ulong currentTimeMs, bool recurse = true) {
// Do forever:
while (true) {
// 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 ? -speed : speed; float leftWheelSpeed = touchRight.touchedSomething ? -600.0f : 600.0f;
// 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 ? -speed : speed; float rightWheelSpeed = touchLeft.touchedSomething ? -600.0f : 600.0f;
// 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
bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
this.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed); // Update the roboid state
bb2b.Update(true);
base.Update(currentTimeMs, recurse); // and sleep for 100ms
Thread.Sleep(100);
} }
} }
} }

55
Examples/BB2B/BB2B_Encoder.cs
View File

@ -1,55 +0,0 @@
using LinearAlgebra;
namespace RoboidControl {
// The robot is based on a differential drive
public class BB2B_Encoder : DifferentialDrive {
readonly DifferentialDrive drive;
readonly TouchSensor touchLeft;
readonly TouchSensor touchRight;
const float speed = 180.0f; // wheel rotation speed in degrees
public BB2B_Encoder(Participant owner) : base(owner) {
this.name = "BB2B";
this.SetDriveDimensions(0.064f, 0.128f);
// Update the basic motors to motors with encoder
EncoderMotor leftMotor = new(this, new RelativeEncoder()) {
position = new Spherical(0.064f, Direction.left)
};
EncoderMotor rightMotor = new(this, new RelativeEncoder()) {
position = new Spherical(0.064f, Direction.right)
};
this.SetMotors(leftMotor, rightMotor);
// Is has a touch sensor at the front left of the roboid
touchLeft = new(this) {
position = Spherical.Degrees(0.12f, -30, 0),
orientation = new SwingTwist(-30, 0, 0)
};
touchRight = new(this) {
position = Spherical.Degrees(0.12f, 30, 0),
orientation = new SwingTwist(30, 0, 0)
};
}
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 leftWheelVelocity = touchRight.touchedSomething ? -speed : speed;
// The right wheel does the same, but instead is controlled by
// touches on the left side
float rightWheelVelocity = touchLeft.touchedSomething ? -speed : speed;
// When both sides are touching something, both wheels will turn backward
// and the roboid will move backwards
this.SetWheelAngularVelocity(leftWheelVelocity, rightWheelVelocity);
base.Update(currentTimeMs, recurse);
}
}
}

13
Examples/BB2B/Program.cs
View File

@ -1,13 +0,0 @@
using System.Threading;
using RoboidControl;
class Program {
static void Main() {
BB2B bb2b = new(ParticipantUDP.Isolated());
while (true) {
bb2b.Update();
Thread.Sleep(100);
}
}
}

146
Unity/DifferentialDrive.cs
View File

@ -1,146 +0,0 @@
#if UNITY_5_3_OR_NEWER
using UnityEngine;
namespace RoboidControl.Unity {
/// <summary>
/// The Unity representation of a Roboid Control differential drive
/// </summary>
public class DifferentialDrive : Thing {
/// <summary>
/// The core differential drive
/// </summary>
public RoboidControl.DifferentialDrive coreDrive => core as RoboidControl.DifferentialDrive;
/// <summary>
/// Create the Unity representation
/// </summary>
/// <param name="coreDrive">The core touch sensor</param>
/// <returns>The Unity representation of the touch sensor</returns>
/// This uses a 'DifferentialDrive' resource when available for the Unity representation.
/// If this is not available, a default representation is created.
public static DifferentialDrive Create(RoboidControl.DifferentialDrive coreDrive) {
DifferentialDrive differentialDrive;
GameObject prefab = (GameObject)Resources.Load("DifferentialDrive");
if (prefab != null) {
// Use resource prefab when available
GameObject gameObj = Instantiate(prefab);
differentialDrive = gameObj.GetComponent<DifferentialDrive>();
differentialDrive.Init(coreDrive);
}
else {
// Default implementation
GameObject gameObj = new(coreDrive.name);
differentialDrive = gameObj.AddComponent<DifferentialDrive>();
differentialDrive.Init(coreDrive);
Rigidbody rb = gameObj.AddComponent<Rigidbody>();
rb.isKinematic = false;
rb.mass = 0.1f;
}
return differentialDrive;
}
/// <summary>
/// The left wheel of the differential drive
/// </summary>
public Wheel leftWheel;
/// <summary>
/// The right wheel of the differential drive
/// </summary>
public Wheel rightWheel;
/// <summary>
/// The caster wheel to keep the differential drive horizontal
/// </summary>
public SphereCollider casterWheel;
/// <summary>
/// The rigidbody of the differential drive
/// </summary>
private Rigidbody rb = null;
/// <summary>
/// Start the Unity representation
/// </summary>
protected virtual void Start() {
rb = GetComponent<Rigidbody>();
}
/// <summary>
/// Handle the binary event indicating a configuration change
/// </summary>
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<Thing>();
foreach (Thing thing in things) {
if (thing.core.id == coreDrive.leftWheel.id)
Destroy(thing.gameObject);
}
}
if (leftWheel == null && coreDrive.leftWheel != null)
leftWheel = Wheel.Create(this.transform, coreDrive.leftWheel.id);
if (leftWheel != null) {
leftWheel.core ??= coreDrive.leftWheel;
SphereCollider leftWheelCollider = leftWheel.GetComponent<SphereCollider>();
leftWheelCollider.radius = coreDrive.wheelRadius;
}
// Destroy any (generic) thing with the same id
if (rightWheel == null) {
Thing[] things = FindObjectsOfType<Thing>();
foreach (Thing thing in things) {
if (thing.core.id == coreDrive.rightWheel.id)
Destroy(thing.gameObject);
}
}
if (rightWheel == null && coreDrive.rightWheel != null)
rightWheel = Wheel.Create(this.transform, coreDrive.rightWheel.id);
if (rightWheel != null) {
rightWheel.core ??= coreDrive.rightWheel;
SphereCollider rightWheelCollider = rightWheel.GetComponent<SphereCollider>();
rightWheelCollider.radius = coreDrive.wheelRadius;
}
if (casterWheel == null) {
GameObject gameObj = new("Caster wheel");
gameObj.transform.parent = this.transform;
casterWheel = gameObj.AddComponent<SphereCollider>();
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);
}
/// <summary>
/// Update the Unity representation state
/// </summary>
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;
// 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;
// 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;
}
}
}
}
#endif

43
Unity/DistanceSensor.cs
View File

@ -5,45 +5,42 @@ using UnityEngine;
namespace RoboidControl.Unity { namespace RoboidControl.Unity {
/// <summary> /// <summary>
/// The Unity representation of a Roboid Control distance sensor /// The Unity representation of a distance sensor
/// </summary> /// </summary>
public class DistanceSensor : Thing { public class DistanceSensor : Thing {
/// <summary> /// <summary>
/// The core distance sensor /// The core distance sensor
/// </summary> /// </summary>
public RoboidControl.DistanceSensor coreSensor => base.core as RoboidControl.DistanceSensor; public new RoboidControl.DistanceSensor core {
get => (RoboidControl.DistanceSensor)base.core;
set => base.core = value;
}
/// <summary> /// <summary>
/// Start the Unity representation /// Start the Unity representation
/// </summary> /// </summary>
protected virtual void Start() { protected virtual void Start() {
if (core == null) {
SiteServer siteServer = FindAnyObjectByType<SiteServer>();
SetCoreThing(new RoboidControl.DistanceSensor(siteServer.site));
}
StartCoroutine(MeasureDistance()); StartCoroutine(MeasureDistance());
} }
/// <summary> /// <summary>
/// Create the Unity representation of the distance sensor /// Create the Unity representation of the distance sensor
/// </summary> /// </summary>
/// <param name="core">The core distance sensor</param> /// <param name="parent">The parent of the core distance sensor</param>
/// <returns>The Unity representation of the distance sensor</returns> /// <returns>The Unity representation of the distance sensor</returns>
/// This uses a 'DistanceSensor' resource when available for the Unity representation. public static DistanceSensor Create(RoboidControl.DistanceSensor core) {
/// If this is not available, a default representation is created. GameObject distanceObj = new("Distance sensor");
public static DistanceSensor Create(RoboidControl.DistanceSensor coreSensor) { DistanceSensor component = distanceObj.AddComponent<DistanceSensor>();
DistanceSensor distanceSensor; if (core.parent != null && core.parent.component != null)
GameObject prefab = (GameObject)Resources.Load("DistanceSensor"); distanceObj.transform.SetParent(core.parent.component.transform, false);
if (prefab != null) {
// Use resource prefab when available return component;
GameObject gameObj = Instantiate(prefab);
distanceSensor = gameObj.GetComponent<DistanceSensor>();
distanceSensor.Init(coreSensor);
}
else {
// Default implementation
GameObject distanceObj = new(coreSensor.name);
distanceSensor = distanceObj.AddComponent<DistanceSensor>();
distanceSensor.Init(coreSensor);
}
return distanceSensor;
} }
/// <summary> /// <summary>
@ -56,10 +53,10 @@ namespace RoboidControl.Unity {
Thing thing = hitInfo.transform.GetComponentInParent<Thing>(); Thing thing = hitInfo.transform.GetComponentInParent<Thing>();
if (thing == null) { if (thing == null) {
// Debug.Log($"collision {hitInfo.transform.name} {hitInfo.distance}"); // Debug.Log($"collision {hitInfo.transform.name} {hitInfo.distance}");
coreSensor.distance = hitInfo.distance; core.distance = hitInfo.distance;
} }
else else
coreSensor.distance = 0; core.distance = 0;
} }
yield return new WaitForSeconds(0.1f); yield return new WaitForSeconds(0.1f);
} }

74
Unity/Motor.cs
View File

@ -1,74 +0,0 @@
#if UNITY_5_3_OR_NEWER
using UnityEngine;
namespace RoboidControl.Unity {
/// <summary>
/// The Unity representation of a Roboid Control motor
/// </summary>
public class Motor : Thing {
/// <summary>
/// The core motor
/// </summary>
public RoboidControl.Motor coreMotor => base.core as RoboidControl.Motor;
/// <summary>
/// Create the Unity representation of the motor
/// </summary>
/// <param name="coreMotor">The core motor</param>
/// <returns>The Unity representation of a motor</returns>
/// This uses a 'Motor' resource when available for the Unity representation.
/// If this is not available, a default representation is created.
public static Motor Create(RoboidControl.Motor coreMotor) {
Motor motor;
GameObject prefab = (GameObject)Resources.Load("Motor");
if (prefab != null) {
// Use resource prefab when available
GameObject gameObj = Instantiate(prefab);
motor = gameObj.GetComponent<Motor>();
motor.Init(coreMotor);
}
else {
// Default implementation
GameObject gameObj = new(coreMotor.name);
motor = gameObj.AddComponent<Motor>();
motor.Init(coreMotor);
Rigidbody rb = gameObj.AddComponent<Rigidbody>();
rb.isKinematic = true;
}
return motor;
}
/// <summary>
/// The maximum rotation speed of the motor in rotations per second
/// </summary>
public float maxSpeed = 5;
/// <summary>
/// The actual rotation speed in rotations per second
/// </summary>
public float rotationSpeed { get; protected set; }
/// <summary>
/// Update the Unity state
/// </summary>
protected override void FixedUpdate() {
base.FixedUpdate();
// We rotate the first child of the motor, which should be the axle.
float rotation = 360 * this.rotationSpeed * Time.fixedDeltaTime;
if (this.transform.childCount > 0)
this.transform.GetChild(0).Rotate(rotation, 0, 0);
}
/// <summary>
/// Handle the binary event containing the rotation speed
/// </summary>
protected override void HandleBinary() {
RoboidControl.Motor coreMotor = core as RoboidControl.Motor;
this.rotationSpeed = coreMotor.targetSpeed * maxSpeed;
}
}
}
#endif

56
Unity/Participant.cs
View File

@ -1,56 +0,0 @@
using UnityEngine;
namespace RoboidControl.Unity {
public class Participant : MonoBehaviour {
public string ipAddress;
public int port;
public RoboidControl.Participant coreParticipant;
protected virtual void Update() {
if (coreParticipant == null)
return;
if (coreParticipant.updateQueue.TryDequeue(out RoboidControl.Participant.UpdateEvent e))
HandleUpdateEvent(e);
}
private void HandleUpdateEvent(RoboidControl.Participant.UpdateEvent e) {
switch (e.messageId) {
case ThingMsg.id:
HandleThingEvent(e);
break;
}
}
protected virtual void HandleThingEvent(RoboidControl.Participant.UpdateEvent e) {
switch (e.thing) {
case RoboidControl.TouchSensor coreTouchSensor:
Debug.Log("Handle TouchSensor");
TouchSensor touchSensor = TouchSensor.Create(coreTouchSensor);
coreTouchSensor.component = touchSensor;
break;
case RoboidControl.DifferentialDrive coreDrive:
Debug.Log("Handle Diff.Drive");
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:
Debug.Log("Handle Thing");
if (coreThing.component == null) {
Thing thing = Thing.Create(coreThing);
coreThing.component = thing;
}
break;
}
}
}
}

73
Unity/RelativeEncoder.cs
View File

@ -1,73 +0,0 @@
#if UNITY_5_3_OR_NEWER
using System.Collections;
using UnityEngine;
using LinearAlgebra;
namespace RoboidControl.Unity {
public class RelativeEncoder : Thing {
/// <summary>
/// Create the Unity representation
/// </summary>
/// <param name="core">The core motor</param>
/// <returns>The Unity representation of a motor</returns>
public static RelativeEncoder Create(RoboidControl.RelativeEncoder core) {
GameObject prefab = (GameObject)Resources.Load("IncrementalEncoder");
if (prefab != null) {
// Use resource prefab when available
GameObject gameObj = Instantiate(prefab);
RelativeEncoder component = gameObj.GetComponent<RelativeEncoder>();
if (component != null)
component.core = core;
return component;
}
else {
// Fallback implementation
GameObject gameObj = new(core.name);
RelativeEncoder component = gameObj.AddComponent<RelativeEncoder>();
component.Init(core);
Rigidbody rb = gameObj.AddComponent<Rigidbody>();
rb.isKinematic = true;
return component;
}
}
RoboidControl.RelativeEncoder coreEncoder => this.core as RoboidControl.RelativeEncoder;
/// <summary>
/// The rotation speed in degrees per second
/// </summary>
public float rotationSpeed = 0.0f;
protected override void HandleBinary() {
this.rotationSpeed = coreEncoder.angularSpeed;
}
private Quaternion lastRotation;
protected override void Update() {
base.Update();
if (this.transform.childCount == 0)
return;
Transform child = this.transform.GetChild(0);
// Get the delta rotation since last frame
Quaternion deltaRotation = Quaternion.Inverse(lastRotation) * child.localRotation;
// Normalize angles to range [-180..180)
// Note: it is not possible to track rotation speeds higher than 180 degrees per frame because of representation limitations of Quaternions
float deltaAngle = Angle.Normalize(deltaRotation.eulerAngles.x);
this.rotationSpeed = deltaAngle / Time.deltaTime;
}
IEnumerator UpdateCore() {
WaitForSeconds wait = new(0.1f);
while (true) {
this.coreEncoder.angularSpeed = this.rotationSpeed;
yield return wait;
}
}
}
}
#endif

546
Unity/Resources/DifferentialDrive_disabled.prefab
View File

@ -1,546 +0,0 @@
%YAML 1.1
%TAG !u! tag:unity3d.com,2011:
--- !u!1 &135949056663158942
GameObject:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
serializedVersion: 6
m_Component:
- component: {fileID: 693610334404048217}
- component: {fileID: 874278397287993211}
- component: {fileID: 8149674613691108646}
- component: {fileID: 7453252590388621785}
m_Layer: 0
m_Name: Body
m_TagString: Untagged
m_Icon: {fileID: 0}
m_NavMeshLayer: 0
m_StaticEditorFlags: 0
m_IsActive: 1
--- !u!4 &693610334404048217
Transform:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 135949056663158942}
serializedVersion: 2
m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
m_LocalPosition: {x: 0, y: 0.01, z: -0.01}
m_LocalScale: {x: 0.12, y: 0.05, z: 0.2}
m_ConstrainProportionsScale: 0
m_Children: []
m_Father: {fileID: 6798369561388671537}
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
--- !u!33 &874278397287993211
MeshFilter:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 135949056663158942}
m_Mesh: {fileID: 10202, guid: 0000000000000000e000000000000000, type: 0}
--- !u!23 &8149674613691108646
MeshRenderer:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 135949056663158942}
m_Enabled: 1
m_CastShadows: 1
m_ReceiveShadows: 1
m_DynamicOccludee: 1
m_StaticShadowCaster: 0
m_MotionVectors: 1
m_LightProbeUsage: 1
m_ReflectionProbeUsage: 1
m_RayTracingMode: 2
m_RayTraceProcedural: 0
m_RenderingLayerMask: 1
m_RendererPriority: 0
m_Materials:
- {fileID: 10303, guid: 0000000000000000f000000000000000, type: 0}
m_StaticBatchInfo:
firstSubMesh: 0
subMeshCount: 0
m_StaticBatchRoot: {fileID: 0}
m_ProbeAnchor: {fileID: 0}
m_LightProbeVolumeOverride: {fileID: 0}
m_ScaleInLightmap: 1
m_ReceiveGI: 1
m_PreserveUVs: 0
m_IgnoreNormalsForChartDetection: 0
m_ImportantGI: 0
m_StitchLightmapSeams: 1
m_SelectedEditorRenderState: 3
m_MinimumChartSize: 4
m_AutoUVMaxDistance: 0.5
m_AutoUVMaxAngle: 89
m_LightmapParameters: {fileID: 0}
m_SortingLayerID: 0
m_SortingLayer: 0
m_SortingOrder: 0
m_AdditionalVertexStreams: {fileID: 0}
--- !u!65 &7453252590388621785
BoxCollider:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 135949056663158942}
m_Material: {fileID: 0}
m_IncludeLayers:
serializedVersion: 2
m_Bits: 0
m_ExcludeLayers:
serializedVersion: 2
m_Bits: 0
m_LayerOverridePriority: 0
m_IsTrigger: 0
m_ProvidesContacts: 0
m_Enabled: 0
serializedVersion: 3
m_Size: {x: 1, y: 1, z: 1}
m_Center: {x: -3.4930155e-36, y: 0, z: 0}
--- !u!1 &3154420579555598162
GameObject:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
serializedVersion: 6
m_Component:
- component: {fileID: 213193100111257669}
- component: {fileID: 894333789048616995}
m_Layer: 0
m_Name: Caster wheel
m_TagString: Untagged
m_Icon: {fileID: 0}
m_NavMeshLayer: 0
m_StaticEditorFlags: 0
m_IsActive: 1
--- !u!4 &213193100111257669
Transform:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 3154420579555598162}
serializedVersion: 2
m_LocalRotation: {x: -0, y: -0, z: 7e-45, w: 1}
m_LocalPosition: {x: 0, y: 0, z: -0.1}
m_LocalScale: {x: 1, y: 1, z: 1}
m_ConstrainProportionsScale: 0
m_Children: []
m_Father: {fileID: 6798369561388671537}
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
--- !u!135 &894333789048616995
SphereCollider:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 3154420579555598162}
m_Material: {fileID: 13400000, guid: 6311a2373d42743449d6eb1130ee618a, type: 2}
m_IncludeLayers:
serializedVersion: 2
m_Bits: 0
m_ExcludeLayers:
serializedVersion: 2
m_Bits: 0
m_LayerOverridePriority: 0
m_IsTrigger: 0
m_ProvidesContacts: 0
m_Enabled: 1
serializedVersion: 3
m_Radius: 0.00001
m_Center: {x: 0, y: 0, z: 0}
--- !u!1 &3377575892836316963
GameObject:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
serializedVersion: 6
m_Component:
- component: {fileID: 7997617267105515189}
- component: {fileID: 6773972788910618332}
- component: {fileID: 3720747953092717687}
m_Layer: 0
m_Name: Left wheel
m_TagString: Untagged
m_Icon: {fileID: 0}
m_NavMeshLayer: 0
m_StaticEditorFlags: 0
m_IsActive: 1
--- !u!4 &7997617267105515189
Transform:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 3377575892836316963}
serializedVersion: 2
m_LocalRotation: {x: -0, y: -0, z: -0, w: 1}
m_LocalPosition: {x: -0.08, y: 0, z: 0}
m_LocalScale: {x: 1, y: 1, z: 1}
m_ConstrainProportionsScale: 0
m_Children:
- {fileID: 9144497505704104684}
m_Father: {fileID: 6798369561388671537}
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
--- !u!114 &6773972788910618332
MonoBehaviour:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 3377575892836316963}
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: ddd5065d5e866894cbcb569c3a898ccb, type: 3}
m_Name:
m_EditorClassIdentifier:
participant: {fileID: 0}
maxSpeed: 5
rotationSpeed: 0
--- !u!135 &3720747953092717687
SphereCollider:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 3377575892836316963}
m_Material: {fileID: 13400000, guid: 6311a2373d42743449d6eb1130ee618a, type: 2}
m_IncludeLayers:
serializedVersion: 2
m_Bits: 0
m_ExcludeLayers:
serializedVersion: 2
m_Bits: 0
m_LayerOverridePriority: 0
m_IsTrigger: 0
m_ProvidesContacts: 0
m_Enabled: 1
serializedVersion: 3
m_Radius: 0.00001
m_Center: {x: 0, y: 0, z: 0}
--- !u!1 &4326386140118987503
GameObject:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
serializedVersion: 6
m_Component:
- component: {fileID: 6798369561388671537}
- component: {fileID: 2975045340952151157}
- component: {fileID: 4118868690347991998}
m_Layer: 0
m_Name: DifferentialDrive_disabled
m_TagString: Untagged
m_Icon: {fileID: 0}
m_NavMeshLayer: 0
m_StaticEditorFlags: 0
m_IsActive: 1
--- !u!4 &6798369561388671537
Transform:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 4326386140118987503}
serializedVersion: 2
m_LocalRotation: {x: 1.276433e-21, y: -1.082074e-19, z: -2.3980766e-36, w: 1}
m_LocalPosition: {x: 2.1932227e-37, y: -0.06941543, z: -1.3084181e-19}
m_LocalScale: {x: 1, y: 1, z: 1}
m_ConstrainProportionsScale: 0
m_Children:
- {fileID: 7997617267105515189}
- {fileID: 2660791421672615984}
- {fileID: 693610334404048217}
- {fileID: 213193100111257669}
m_Father: {fileID: 0}
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
--- !u!114 &2975045340952151157
MonoBehaviour:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 4326386140118987503}
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: 5c80bc2fd1d8aa14facb1ad4b6ccf7b3, type: 3}
m_Name:
m_EditorClassIdentifier:
participant: {fileID: 0}
leftWheel: {fileID: 6773972788910618332}
rightWheel: {fileID: 7425139233737877139}
casterWheel: {fileID: 894333789048616995}
--- !u!54 &4118868690347991998
Rigidbody:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 4326386140118987503}
serializedVersion: 4
m_Mass: 0.1
m_Drag: 0
m_AngularDrag: 0.05
m_CenterOfMass: {x: 0, y: 0, z: 0}
m_InertiaTensor: {x: 1, y: 1, z: 1}
m_InertiaRotation: {x: 0, y: 0, z: 0, w: 1}
m_IncludeLayers:
serializedVersion: 2
m_Bits: 0
m_ExcludeLayers:
serializedVersion: 2
m_Bits: 0
m_ImplicitCom: 1
m_ImplicitTensor: 1
m_UseGravity: 1
m_IsKinematic: 0
m_Interpolate: 0
m_Constraints: 0
m_CollisionDetection: 0
--- !u!1 &4927629757198460283
GameObject:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
serializedVersion: 6
m_Component:
- component: {fileID: 9144497505704104684}
- component: {fileID: 8353236605004633556}
- component: {fileID: 1652944855748388758}
m_Layer: 0
m_Name: Cube
m_TagString: Untagged
m_Icon: {fileID: 0}
m_NavMeshLayer: 0
m_StaticEditorFlags: 0
m_IsActive: 1
--- !u!4 &9144497505704104684
Transform:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 4927629757198460283}
serializedVersion: 2
m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
m_LocalPosition: {x: 0, y: 0, z: 0}
m_LocalScale: {x: 0.01, y: 0.05, z: 0.05}
m_ConstrainProportionsScale: 0
m_Children: []
m_Father: {fileID: 7997617267105515189}
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
--- !u!33 &8353236605004633556
MeshFilter:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 4927629757198460283}
m_Mesh: {fileID: 10202, guid: 0000000000000000e000000000000000, type: 0}
--- !u!23 &1652944855748388758
MeshRenderer:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 4927629757198460283}
m_Enabled: 1
m_CastShadows: 1
m_ReceiveShadows: 1
m_DynamicOccludee: 1
m_StaticShadowCaster: 0
m_MotionVectors: 1
m_LightProbeUsage: 1
m_ReflectionProbeUsage: 1
m_RayTracingMode: 2
m_RayTraceProcedural: 0
m_RenderingLayerMask: 1
m_RendererPriority: 0
m_Materials:
- {fileID: 2100000, guid: c83c86eecb501964d9e4fd4074683016, type: 2}
m_StaticBatchInfo:
firstSubMesh: 0
subMeshCount: 0
m_StaticBatchRoot: {fileID: 0}
m_ProbeAnchor: {fileID: 0}
m_LightProbeVolumeOverride: {fileID: 0}
m_ScaleInLightmap: 1
m_ReceiveGI: 1
m_PreserveUVs: 0
m_IgnoreNormalsForChartDetection: 0
m_ImportantGI: 0
m_StitchLightmapSeams: 1
m_SelectedEditorRenderState: 3
m_MinimumChartSize: 4
m_AutoUVMaxDistance: 0.5
m_AutoUVMaxAngle: 89
m_LightmapParameters: {fileID: 0}
m_SortingLayerID: 0
m_SortingLayer: 0
m_SortingOrder: 0
m_AdditionalVertexStreams: {fileID: 0}
--- !u!1 &5122915782100933114
GameObject:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
serializedVersion: 6
m_Component:
- component: {fileID: 2660791421672615984}
- component: {fileID: 7425139233737877139}
- component: {fileID: 8150853676422519226}
m_Layer: 0
m_Name: Right wheel
m_TagString: Untagged
m_Icon: {fileID: 0}
m_NavMeshLayer: 0
m_StaticEditorFlags: 0
m_IsActive: 1
--- !u!4 &2660791421672615984
Transform:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 5122915782100933114}
serializedVersion: 2
m_LocalRotation: {x: -0, y: -0, z: -0, w: 1}
m_LocalPosition: {x: 0.08, y: 0, z: 0}
m_LocalScale: {x: 1, y: 1, z: 1}
m_ConstrainProportionsScale: 0
m_Children:
- {fileID: 2324127387642107303}
m_Father: {fileID: 6798369561388671537}
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
--- !u!114 &7425139233737877139
MonoBehaviour:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 5122915782100933114}
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: ddd5065d5e866894cbcb569c3a898ccb, type: 3}
m_Name:
m_EditorClassIdentifier:
participant: {fileID: 0}
maxSpeed: 5
rotationSpeed: 0
--- !u!135 &8150853676422519226
SphereCollider:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 5122915782100933114}
m_Material: {fileID: 13400000, guid: 6311a2373d42743449d6eb1130ee618a, type: 2}
m_IncludeLayers:
serializedVersion: 2
m_Bits: 0
m_ExcludeLayers:
serializedVersion: 2
m_Bits: 0
m_LayerOverridePriority: 0
m_IsTrigger: 0
m_ProvidesContacts: 0
m_Enabled: 1
serializedVersion: 3
m_Radius: 0.00001
m_Center: {x: 0, y: 0, z: 0}
--- !u!1 &5663058822615269368
GameObject:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
serializedVersion: 6
m_Component:
- component: {fileID: 2324127387642107303}
- component: {fileID: 7927909525546489266}
- component: {fileID: 1038362976985791207}
m_Layer: 0
m_Name: Cube
m_TagString: Untagged
m_Icon: {fileID: 0}
m_NavMeshLayer: 0
m_StaticEditorFlags: 0
m_IsActive: 1
--- !u!4 &2324127387642107303
Transform:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 5663058822615269368}
serializedVersion: 2
m_LocalRotation: {x: -0, y: -0, z: 7e-45, w: 1}
m_LocalPosition: {x: 0, y: 0, z: 0}
m_LocalScale: {x: 0.01, y: 0.049999997, z: 0.049999997}
m_ConstrainProportionsScale: 0
m_Children: []
m_Father: {fileID: 2660791421672615984}
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
--- !u!33 &7927909525546489266
MeshFilter:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 5663058822615269368}
m_Mesh: {fileID: 10202, guid: 0000000000000000e000000000000000, type: 0}
--- !u!23 &1038362976985791207
MeshRenderer:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 5663058822615269368}
m_Enabled: 1
m_CastShadows: 1
m_ReceiveShadows: 1
m_DynamicOccludee: 1
m_StaticShadowCaster: 0
m_MotionVectors: 1
m_LightProbeUsage: 1
m_ReflectionProbeUsage: 1
m_RayTracingMode: 2
m_RayTraceProcedural: 0
m_RenderingLayerMask: 1
m_RendererPriority: 0
m_Materials:
- {fileID: 2100000, guid: c83c86eecb501964d9e4fd4074683016, type: 2}
m_StaticBatchInfo:
firstSubMesh: 0
subMeshCount: 0
m_StaticBatchRoot: {fileID: 0}
m_ProbeAnchor: {fileID: 0}
m_LightProbeVolumeOverride: {fileID: 0}
m_ScaleInLightmap: 1
m_ReceiveGI: 1
m_PreserveUVs: 0
m_IgnoreNormalsForChartDetection: 0
m_ImportantGI: 0
m_StitchLightmapSeams: 1
m_SelectedEditorRenderState: 3
m_MinimumChartSize: 4
m_AutoUVMaxDistance: 0.5
m_AutoUVMaxAngle: 89
m_LightmapParameters: {fileID: 0}
m_SortingLayerID: 0
m_SortingLayer: 0
m_SortingOrder: 0
m_AdditionalVertexStreams: {fileID: 0}

14
Unity/Resources/SlidingWheel.physicMaterial
View File

@ -1,14 +0,0 @@
%YAML 1.1
%TAG !u! tag:unity3d.com,2011:
--- !u!134 &13400000
PhysicMaterial:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_Name: SlidingWheel
dynamicFriction: 0.02
staticFriction: 0.01
bounciness: 0
frictionCombine: 1
bounceCombine: 0

191
Unity/Resources/TouchSensor.prefab
View File

@ -1,191 +0,0 @@
%YAML 1.1
%TAG !u! tag:unity3d.com,2011:
--- !u!1 &1535520457298351474
GameObject:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
serializedVersion: 6
m_Component:
- component: {fileID: 1122722937167266542}
- component: {fileID: 8409364771467476437}
- component: {fileID: 6632295589629461102}
m_Layer: 0
m_Name: Sphere
m_TagString: Untagged
m_Icon: {fileID: 0}
m_NavMeshLayer: 0
m_StaticEditorFlags: 0
m_IsActive: 1
--- !u!4 &1122722937167266542
Transform:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 1535520457298351474}
serializedVersion: 2
m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
m_LocalPosition: {x: 0, y: 0, z: 0}
m_LocalScale: {x: 0.01, y: 0.01, z: 0.01}
m_ConstrainProportionsScale: 0
m_Children: []
m_Father: {fileID: 839661735326876684}
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
--- !u!33 &8409364771467476437
MeshFilter:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 1535520457298351474}
m_Mesh: {fileID: 10207, guid: 0000000000000000e000000000000000, type: 0}
--- !u!23 &6632295589629461102
MeshRenderer:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 1535520457298351474}
m_Enabled: 1
m_CastShadows: 1
m_ReceiveShadows: 1
m_DynamicOccludee: 1
m_StaticShadowCaster: 0
m_MotionVectors: 1
m_LightProbeUsage: 1
m_ReflectionProbeUsage: 1
m_RayTracingMode: 2
m_RayTraceProcedural: 0
m_RenderingLayerMask: 1
m_RendererPriority: 0
m_Materials:
- {fileID: 10303, guid: 0000000000000000f000000000000000, type: 0}
m_StaticBatchInfo:
firstSubMesh: 0
subMeshCount: 0
m_StaticBatchRoot: {fileID: 0}
m_ProbeAnchor: {fileID: 0}
m_LightProbeVolumeOverride: {fileID: 0}
m_ScaleInLightmap: 1
m_ReceiveGI: 1
m_PreserveUVs: 0
m_IgnoreNormalsForChartDetection: 0
m_ImportantGI: 0
m_StitchLightmapSeams: 1
m_SelectedEditorRenderState: 3
m_MinimumChartSize: 4
m_AutoUVMaxDistance: 0.5
m_AutoUVMaxAngle: 89
m_LightmapParameters: {fileID: 0}
m_SortingLayerID: 0
m_SortingLayer: 0
m_SortingOrder: 0
m_AdditionalVertexStreams: {fileID: 0}
--- !u!1 &6521541507066528382
GameObject:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
serializedVersion: 6
m_Component:
- component: {fileID: 839661735326876684}
- component: {fileID: 1922171662874522792}
- component: {fileID: 5295705618145761524}
- component: {fileID: 5884202444353424411}
m_Layer: 0
m_Name: TouchSensor
m_TagString: Untagged
m_Icon: {fileID: 0}
m_NavMeshLayer: 0
m_StaticEditorFlags: 0
m_IsActive: 1
--- !u!4 &839661735326876684
Transform:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 6521541507066528382}
serializedVersion: 2
m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
m_LocalPosition: {x: 0, y: -0, z: 0}
m_LocalScale: {x: 1, y: 1, z: 1}
m_ConstrainProportionsScale: 0
m_Children:
- {fileID: 1122722937167266542}
m_Father: {fileID: 0}
m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
--- !u!114 &1922171662874522792
MonoBehaviour:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 6521541507066528382}
m_Enabled: 1
m_EditorHideFlags: 0
m_Script: {fileID: 11500000, guid: ab969221c3007d441b18dd7387cf22d4, type: 3}
m_Name:
m_EditorClassIdentifier:
<core>k__BackingField:
terminate: 0
id: 4
type: 5
nameChanged: 1
hierarchyChanged: 1
positionUpdated: 0
orientationUpdated: 0
linearVelocityUpdated: 0
angularVelocityUpdated: 0
participant: {fileID: 0}
--- !u!54 &5295705618145761524
Rigidbody:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 6521541507066528382}
serializedVersion: 4
m_Mass: 1
m_Drag: 0
m_AngularDrag: 0.05
m_CenterOfMass: {x: 0, y: 0, z: 0}
m_InertiaTensor: {x: 1, y: 1, z: 1}
m_InertiaRotation: {x: 0, y: 0, z: 0, w: 1}
m_IncludeLayers:
serializedVersion: 2
m_Bits: 0
m_ExcludeLayers:
serializedVersion: 2
m_Bits: 0
m_ImplicitCom: 1
m_ImplicitTensor: 1
m_UseGravity: 1
m_IsKinematic: 1
m_Interpolate: 0
m_Constraints: 0
m_CollisionDetection: 0
--- !u!135 &5884202444353424411
SphereCollider:
m_ObjectHideFlags: 0
m_CorrespondingSourceObject: {fileID: 0}
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 6521541507066528382}
m_Material: {fileID: 0}
m_IncludeLayers:
serializedVersion: 2
m_Bits: 0
m_ExcludeLayers:
serializedVersion: 2
m_Bits: 0
m_LayerOverridePriority: 0
m_IsTrigger: 1
m_ProvidesContacts: 0
m_Enabled: 1
serializedVersion: 3
m_Radius: 0.01
m_Center: {x: 0, y: 0, z: 0}

40
Unity/SiteServer.cs
View File

@ -5,17 +5,18 @@ using UnityEngine;
namespace RoboidControl.Unity { namespace RoboidControl.Unity {
public class SiteServer : Participant { public class SiteServer : MonoBehaviour {
public int port = 7681;
//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());
this.coreParticipant = new RoboidControl.SiteServer(port); site = new RoboidControl.SiteServer(port);
RoboidControl.Thing.OnNewThing += HandleNewThing;
} }
void OnApplicationQuit() { void OnApplicationQuit() {
@ -23,29 +24,16 @@ namespace RoboidControl.Unity {
site.Close(); site.Close();
} }
protected override void Update() { public void HandleNewThing(RoboidControl.Thing thing) {
if (site == null) //Debug.Log($"Handle New thing event for {thing}");
return; //site.Add(thing, false);
thingQueue.Enqueue(thing);
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();
} }
private void HandleUpdateEvent(RoboidControl.Participant.UpdateEvent e) { protected virtual void Update() {
switch (e.messageId) { site.Update((ulong)(Time.time * 1000));
case ParticipantMsg.Id: while (thingQueue.TryDequeue(out RoboidControl.Thing thing))
GameObject remoteParticipant = new GameObject("RemoteParticipant"); thing.CreateComponent();
Participant participant = remoteParticipant.AddComponent<Participant>();
participant.coreParticipant = e.participant;
break;
case ThingMsg.id:
HandleThingEvent(e);
break;
}
} }
} }

233
Unity/Thing.cs
View File

@ -2,32 +2,40 @@
using System.Collections; using System.Collections;
using UnityEngine; using UnityEngine;
using UnityEngine.Networking; using UnityEngine.Networking;
#if GLTF
using GLTFast;
#endif
namespace RoboidControl.Unity { namespace RoboidControl.Unity {
/// <summary> /// <summary>
/// The Unity representation fo a Roboid Control Thing /// The representation of a Thing in Unity
/// </summary> /// </summary>
public class Thing : MonoBehaviour { public class Thing : MonoBehaviour {
/// <summary> /// <summary>
/// The core C# thing /// The core C# thing
/// </summary> /// </summary>
[field: SerializeField]
public RoboidControl.Thing core { get; set; } public RoboidControl.Thing core { get; set; }
/// <summary> public SiteServer participant;
/// The owner of this thing
/// </summary> private string modelUrl = null;
public Participant owner;
/// <summary> /// <summary>
/// Create a Unity representation of a Thing /// Set the core C# thing
/// </summary> /// </summary>
/// <param name="core">The core of the thing</param> protected void SetCoreThing(RoboidControl.Thing thing) {
/// <returns>The created thing</returns> core = thing;
core.component = this;
SiteServer siteServer = FindAnyObjectByType<SiteServer>();
if (siteServer == null || siteServer.site == null) {
Debug.LogWarning("No site server found");
return;
}
siteServer.site.Add(thing);
core.OnPoseChanged += PoseChanged;
}
public static Thing Create(RoboidControl.Thing core) { public static Thing Create(RoboidControl.Thing core) {
// Debug.Log("Creating new Unity thing"); // Debug.Log("Creating new Unity thing");
GameObject gameObj = string.IsNullOrEmpty(core.name) ? GameObject gameObj = string.IsNullOrEmpty(core.name) ?
@ -38,34 +46,29 @@ namespace RoboidControl.Unity {
return component; return component;
} }
/// <summary>
/// Initialize the Thing
/// </summary>
/// <param name="core">The core of the thing</param>
/// This affects the parent and pose of the thing
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.owner = FindAnyObjectByType<SiteServer>(); if (core.parent != null && core.parent.component != null)
core.owner = this.owner.coreParticipant;
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();
if (core.orientation != null) if (core.orientation != null)
this.transform.localRotation = core.orientation.ToQuaternion(); this.transform.localRotation = core.orientation.ToQuaternion();
core.OnPoseChanged += this.PoseChanged;
} }
/// <summary> /// <summary>
/// Update the Unity rendering /// Update the Unity representation
/// </summary> /// </summary>
protected virtual void Update() { protected virtual void Update() {
if (core == null) if (core == null) {
// Debug.Log("Core thing is gone, self destruct in 0 seconds...");
Destroy(this);
return; return;
}
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;
@ -76,69 +79,38 @@ namespace RoboidControl.Unity {
Vector3 axis = core.angularVelocity.direction.ToVector3(); Vector3 axis = core.angularVelocity.direction.ToVector3();
this.transform.localRotation *= Quaternion.AngleAxis(core.angularVelocity.distance * Time.deltaTime, axis); this.transform.localRotation *= Quaternion.AngleAxis(core.angularVelocity.distance * Time.deltaTime, axis);
} }
}
/// <summary> if (!string.IsNullOrEmpty(core.modelUrl) && this.modelUrl == null) {
/// Update the Unity state (just calls UpdateThing) string extension = core.modelUrl.Substring(core.modelUrl.LastIndexOf("."));
/// </summary> if (extension == ".jpg" || extension == ".png") {
protected virtual void FixedUpdate() { StartCoroutine(LoadJPG());
UpdateThing(); }
}
/// <summary> this.modelUrl = core.modelUrl;
/// Update the Unity state
/// </summary>
public void UpdateThing() {
if (core == null) {
Debug.Log($"{this} core thing is gone, self destruct in 0 seconds...");
Destroy(this);
return;
} }
if (core.nameChanged) {
while (core.updateQueue.TryDequeue(out RoboidControl.Thing.CoreEvent e)) if (this.gameObject.name != core.name)
HandleCoreEvent(e);
}
/// <summary>
/// Handle events from the core thing
/// </summary>
/// <param name="coreEvent">The core event to handle</param>
private void HandleCoreEvent(RoboidControl.Thing.CoreEvent coreEvent) {
switch (coreEvent.messageId) {
case ThingMsg.id:
Debug.Log($"{this.core.id} Handle Thing");
if (core.parent == null)
this.transform.SetParent(null, true);
else if (core.parent.component != null)
this.transform.SetParent(core.parent.component.transform, true);
break;
case NameMsg.Id:
Debug.Log($"{this.core.id} Handle Name");
this.gameObject.name = core.name; this.gameObject.name = core.name;
break; core.nameChanged = false;
case ModelUrlMsg.Id: }
Debug.Log("{this.id} Handle Model URL"); if (core.hierarchyChanged) {
string extension = core.modelUrl[core.modelUrl.LastIndexOf(".")..]; // Debug.Log("Parent changed");
if (extension == ".jpg" || extension == ".png") if (core.parent == null)
StartCoroutine(LoadJPG()); this.transform.SetParent(null, true);
else if (extension == ".gltf" || extension == ".glb") else
ProcessGltfModel(core); this.transform.SetParent(core.parent.component.transform, true);
break; core.hierarchyChanged = false;
case PoseMsg.Id:
Debug.Log($"{this.core.id} Handle Pose");
this.HandlePose();
break;
case BinaryMsg.Id:
Debug.Log($"{this.core.id} Handle Binary");
this.HandleBinary();
break;
} }
} }
/// <summary> private void PoseChanged() {
/// Load and attach a JPG sprite visualization of the thing //Debug.Log($"{this} pose changed");
/// </summary> if (core.positionUpdated)
/// <returns></returns> 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();
@ -166,105 +138,6 @@ namespace RoboidControl.Unity {
} }
} }
bool loadingModel = false;
private async void ProcessGltfModel(RoboidControl.Thing coreThing) {
#if GLTF
if (!loadingModel) {
loadingModel = true;
Debug.Log("Loading GLTF model from :" + coreThing.modelUrl);
GltfImport gltfImport = new GltfImport();
bool success = await gltfImport.Load(coreThing.modelUrl);
if (success) {
Transform parentTransform = this.transform;
Thing[] things = FindObjectsOfType<Thing>();
Thing parentThing = null;
foreach (Thing thing in things) {
if (thing.core.id == coreThing.id) {
parentTransform = thing.transform;
parentThing = thing;
}
}
await gltfImport.InstantiateMainSceneAsync(parentTransform);
SkinnedMeshRenderer[] meshRenderers = parentTransform.GetComponentsInChildren<SkinnedMeshRenderer>();
#if pHUMANOID4
if (parentThing.objectType == 7) {
HumanoidControl hc = parentThing.gameObject.GetComponent<HumanoidControl>();
if (hc == null)
hc = parentThing.gameObject.AddComponent<HumanoidControl>();
foreach (SkinnedMeshRenderer meshRenderer in meshRenderers) {
if (meshRenderer.rootBone != null) {
Debug.Log("Found a skinned mesh with bones");
hc.RetrieveBonesFrom(meshRenderer.rootBone);
break;
}
}
}
#endif
parentTransform.localScale = Vector3.one;
if (meshRenderers.Length > 0) {
foreach (SkinnedMeshRenderer meshRenderer in meshRenderers) {
if (meshRenderer.rootBone != null) {
Debug.Log("Found a skinned mesh with bones");
ScanForThings(meshRenderer.rootBone);
break;
}
}
}
else {
ScanForThings(parentTransform);
}
}
else {
this.transform.localScale = Vector3.one * 1;
}
}
loadingModel = true;
#endif
}
private void ScanForThings(Transform rootTransform) {
// Thing[] thingArray = allThings.ToArray();
// for (int thingIx = 0; thingIx < thingArray.Length; thingIx++) {
// Thing thing = thingArray[thingIx];
// GameObject foundObj = FindThingByName(thing, rootTransform);
// if (foundObj != null && foundObj != thing.gameObject) {
// Thing foundThing = foundObj.GetComponent<Thing>();
// if (foundThing == null) {
// allThings.Remove(thing);
// foundThing = foundObj.AddComponent<Thing>();
// foundThing.networkId = thing.networkId;
// foundThing.objectId = thing.objectId;
// allThings.Add(foundThing);
// Destroy(thing.gameObject);
// }
// }
// }
}
/// <summary>
/// Handle a Pose event
/// </summary>
/// This can update the position and/or orientation when the velocity of the thing is zero.
/// If a velocity is not zero, the position and/or orientation update will be ignored
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();
}
/// <summary>
/// Handle a Binary event
/// </summary>
protected virtual void HandleBinary() { }
} }

95
Unity/TouchSensor.cs
View File

@ -4,70 +4,87 @@ using UnityEngine;
namespace RoboidControl.Unity { namespace RoboidControl.Unity {
/// <summary> /// <summary>
/// The Unity representation of a Roboid Control touch sensor /// The Unity representation of the TouchSensor
/// </summary> /// </summary>
public class TouchSensor : Thing { public class TouchSensor : Thing {
// public SiteServer participant;
/// <summary> /// <summary>
/// The core touch sensor /// The core touch sensor
/// </summary> /// </summary>
public RoboidControl.TouchSensor coreSensor => base.core as RoboidControl.TouchSensor; public RoboidControl.TouchSensor coreSensor {
get => (RoboidControl.TouchSensor)base.core;
}
SphereCollider collider = null;
/// <summary> /// <summary>
/// Create the Unity representation of the touch sensor /// Start the Unity represention
/// </summary> /// </summary>
/// <param name="coreSensor">The core touch sensor</param> protected virtual void Start() {
if (core == null) {
participant = FindAnyObjectByType<SiteServer>();
SetCoreThing(new RoboidControl.TouchSensor(participant.site));
}
collider = GetComponent<SphereCollider>();
}
/// <summary>
/// Create the Unity representation
/// </summary>
/// <param name="core">The core touch sensor</param>
/// <returns>The Unity representation of the touch sensor</returns> /// <returns>The Unity representation of the touch sensor</returns>
/// This uses a 'TouchSensor' resource when available for the Unity representation. public static TouchSensor Create(RoboidControl.TouchSensor core) {
/// If this is not available, a default representation is created. GameObject gameObj = core.name != null ?
public static TouchSensor Create(RoboidControl.TouchSensor coreSensor) { new(core.name) :
TouchSensor touchSensor; new("Touch Sensor");
GameObject prefab = (GameObject)Resources.Load("TouchSensor"); TouchSensor component = gameObj.AddComponent<TouchSensor>();
if (prefab != null) { component.Init(core);
// Use resource prefab when available
GameObject gameObj = Instantiate(prefab);
touchSensor = gameObj.GetComponent<TouchSensor>();
touchSensor.Init(coreSensor);
}
else {
// Default implementation
GameObject gameObj = new(coreSensor.name);
touchSensor = gameObj.AddComponent<TouchSensor>();
touchSensor.Init(coreSensor);
Rigidbody rb = gameObj.AddComponent<Rigidbody>(); Rigidbody rb = gameObj.AddComponent<Rigidbody>();
rb.isKinematic = true; rb.isKinematic = true;
SphereCollider collider = gameObj.AddComponent<SphereCollider>(); SphereCollider collider = gameObj.AddComponent<SphereCollider>();
collider.radius = 0.01f; collider.radius = 0.01f;
collider.isTrigger = true; collider.isTrigger = true;
if (gameObj.transform.parent != null && gameObj.transform.localPosition.magnitude > 0) {
collider.radius = Vector3.Distance(gameObj.transform.position, gameObj.transform.parent.position) / 2;
gameObj.transform.position = (gameObj.transform.position + gameObj.transform.parent.position) / 2;
}
return component;
}
protected override void Update() {
base.Update();
if (collider.radius == 0.01f &&
this.transform.parent != null && this.transform.localPosition.magnitude > 0
) {
collider.radius = Vector3.Distance(this.transform.position, this.transform.parent.position) / 2;
this.transform.position = (this.transform.position + this.transform.parent.position) / 2;
} }
return touchSensor;
} }
/// <summary>
/// Handle touch trigger collider enter event
/// </summary>
/// <param name="other">The collider which entered our trigger collider</param>
private void OnTriggerEnter(Collider other) { private void OnTriggerEnter(Collider other) {
// Don't detect trigger colliders // Debug.Log("Touch?");
if (other.isTrigger) if (other.isTrigger) {
// Debug.Log($" was trigger {other.name}");
return; return;
// Don't touch yourself }
if (this.transform.root == other.transform.root) if (this.transform.root == other.transform.root) {
Debug.Log($" was myself {other.name}");
return; return;
}
Debug.Log($"*** {this} Touch");
this.coreSensor.touchedSomething = true; this.coreSensor.touchedSomething = true;
this.core.updateQueue.Enqueue(new RoboidControl.Thing.CoreEvent(BinaryMsg.Id));
} }
/// <summary>
/// Handl touch trigger collider exit event
/// </summary>
/// <param name="other">The collider which exited our trigger collider </param>
private void OnTriggerExit(Collider other) { private void OnTriggerExit(Collider other) {
if (other.isTrigger) if (other.isTrigger)
return; return;
Debug.Log($"*** {this} Touch end");
this.coreSensor.touchedSomething = false; this.coreSensor.touchedSomething = false;
} }
} }

85
Unity/Wheel.cs
View File

@ -1,85 +0,0 @@
#if UNITY_5_3_OR_NEWER
using UnityEngine;
namespace RoboidControl.Unity {
/// <summary>
/// The Unity representation of a Roboid Control wheel
/// </summary>
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);
Debug.Log("Create " + core.name);
return component;
}
}
public static Wheel Create(Transform parent, 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 = parent;
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);
SphereCollider collider = gameObj.AddComponent<SphereCollider>();
collider.radius = 0.00001f;
collider.material = slidingWheel;
Debug.Log("Create placeholder Wheel ");
return component;
}
}
private static PhysicMaterial _slidingWheel;
public static PhysicMaterial slidingWheel {
get {
if (_slidingWheel == null) {
_slidingWheel = new() {
dynamicFriction = 0.02f, // Dynamic friction
staticFriction = 0.01f, // Static friction
bounciness = 0f, // Bounciness (bounciness value between 0 and 1)
// Set the friction combine and bounce combine methods
frictionCombine = PhysicMaterialCombine.Minimum, // How to combine friction
bounceCombine = PhysicMaterialCombine.Average // How to combine bounciness
};
}
return _slidingWheel;
}
}
}
}
#endif

15
src/Messages/BinaryMsg.cs
View File

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

45
src/Participant.cs
View File

@ -1,8 +1,5 @@
using System; using System;
using System.Collections.Generic; using System.Collections.Generic;
using System.Collections.Concurrent;
using System.Net;
using System.Net.Sockets;
namespace RoboidControl { namespace RoboidControl {
@ -19,11 +16,9 @@ namespace RoboidControl {
/// </summary> /// </summary>
/// <param name="ipAddress">The IP address of the participant</param> /// <param name="ipAddress">The IP address of the participant</param>
/// <param name="port">The UDP port of the participant</param> /// <param name="port">The UDP port of the participant</param>
public Participant(string ipAddress, int port, Participant localParticipant = null) { public Participant(string ipAddress, int port) {
this.ipAddress = ipAddress; this.ipAddress = ipAddress;
this.port = port; this.port = port;
if (localParticipant != null)
this.udpClient = localParticipant.udpClient;
} }
/// <summary> /// <summary>
@ -35,8 +30,6 @@ namespace RoboidControl {
/// </summary> /// </summary>
public int port = 0; public int port = 0;
public UdpClient udpClient = null;
/// <summary> /// <summary>
/// he network Id to identify the participant /// he network Id to identify the participant
/// </summary> /// </summary>
@ -85,8 +78,6 @@ namespace RoboidControl {
this.things.Remove(thing); this.things.Remove(thing);
} }
#region Update
/// <summary> /// <summary>
/// Update all things for this participant /// Update all things for this participant
/// </summary> /// </summary>
@ -100,36 +91,6 @@ namespace RoboidControl {
} }
} }
public class UpdateEvent {
public int messageId; // see the communication messages
public Thing thing;
public Participant participant;
}
public ConcurrentQueue<UpdateEvent> updateQueue = new();
#endregion Update
#region Send
// Would be nice if this could be shared between all participants....
public byte[] buffer = new byte[1024];
public virtual bool Send(IMessage msg) {
int bufferSize = msg.Serialize(ref this.buffer);
if (bufferSize <= 0)
return true;
IPEndPoint participantEndpoint = new IPEndPoint(IPAddress.Parse(this.ipAddress), this.port);
Console.WriteLine($"msg to remote participant {participantEndpoint.Address.ToString()} {participantEndpoint.Port}");
if (udpClient != null) {
Console.WriteLine("sending...");
this.udpClient?.Send(this.buffer, bufferSize, participantEndpoint);
}
return true;
}
#endregion Send
/// <summary> /// <summary>
/// The collection of known participants. /// The collection of known participants.
/// </summary> /// </summary>
@ -169,9 +130,9 @@ namespace RoboidControl {
/// <param name="ipAddress">The IP address of the participant</param> /// <param name="ipAddress">The IP address of the participant</param>
/// <param name="port">The port used to send messages to this participant</param> /// <param name="port">The port used to send messages to this participant</param>
/// <returns>The added participant</returns> /// <returns>The added participant</returns>
public static Participant AddParticipant(string ipAddress, int port, Participant localParticipant = null) { public static Participant AddParticipant(string ipAddress, int port) {
Console.WriteLine($"New Participant {ipAddress}:{port}"); Console.WriteLine($"New Participant {ipAddress}:{port}");
Participant participant = new(ipAddress, port, localParticipant) { Participant participant = new(ipAddress, port) {
networkId = (byte)(Participant.participants.Count + 1) networkId = (byte)(Participant.participants.Count + 1)
}; };
Participant.participants.Add(participant); Participant.participants.Add(participant);

81
src/Participants/ParticipantUDP.cs
View File

@ -43,6 +43,13 @@ namespace RoboidControl {
/// <param name="port">The port number of the site server</param> /// <param name="port">The port number of the site server</param>
/// <param name="localPort">The port used by the local participant</param> /// <param name="localPort">The port used by the local participant</param>
public ParticipantUDP(string ipAddress, int port = 7681, int localPort = 7681) : base("127.0.0.1", localPort) { public ParticipantUDP(string ipAddress, int port = 7681, int localPort = 7681) : base("127.0.0.1", localPort) {
if (this.port == 0)
this.isIsolated = true;
else
this.remoteSite = new Participant(ipAddress, port);
Participant.AddParticipant(this);
// Determine local IP address // Determine local IP address
IPAddress localIpAddress = null; IPAddress localIpAddress = null;
IPAddress subnetMask = null; IPAddress subnetMask = null;
@ -73,12 +80,6 @@ namespace RoboidControl {
this.udpClient = new UdpClient(localPort); this.udpClient = new UdpClient(localPort);
this.udpClient.BeginReceive(new AsyncCallback(result => ReceiveUDP(result)), null); this.udpClient.BeginReceive(new AsyncCallback(result => ReceiveUDP(result)), null);
if (this.port == 0)
this.isIsolated = true;
else
this.remoteSite = new Participant(ipAddress, port, this);
Participant.AddParticipant(this);
} }
private static ParticipantUDP isolatedParticipant = null; private static ParticipantUDP isolatedParticipant = null;
@ -115,10 +116,10 @@ namespace RoboidControl {
/// </summary> /// </summary>
public ulong publishInterval = 3000; // = 3 seconds public ulong publishInterval = 3000; // = 3 seconds
//public byte[] buffer = new byte[1024]; public byte[] buffer = new byte[1024];
public IPEndPoint endPoint = null; public IPEndPoint endPoint = null;
//public UdpClient udpClient = null; public UdpClient udpClient = null;
public string broadcastIpAddress = "255.255.255.255"; public string broadcastIpAddress = "255.255.255.255";
public readonly ConcurrentQueue<IMessage> messageQueue = new ConcurrentQueue<IMessage>(); public readonly ConcurrentQueue<IMessage> messageQueue = new ConcurrentQueue<IMessage>();
@ -156,8 +157,7 @@ namespace RoboidControl {
if (this.remoteSite == null) if (this.remoteSite == null)
this.Publish(msg); this.Publish(msg);
else else
//this.Send(this.remoteSite, msg); this.Send(this.remoteSite, msg);
this.remoteSite.Send(msg);
this.nextPublishMe = currentTimeMS + this.publishInterval; this.nextPublishMe = currentTimeMS + this.publishInterval;
} }
@ -174,8 +174,7 @@ namespace RoboidControl {
if (thing.hierarchyChanged && !(this.isIsolated || this.networkId == 0)) { if (thing.hierarchyChanged && !(this.isIsolated || this.networkId == 0)) {
ThingMsg thingMsg = new(this.networkId, thing); ThingMsg thingMsg = new(this.networkId, thing);
// this.Send(this.remoteSite, thingMsg); this.Send(this.remoteSite, thingMsg);
this.remoteSite.Send(thingMsg);
} }
// Why don't we do recursive? // Why don't we do recursive?
@ -186,17 +185,14 @@ namespace RoboidControl {
if (!(this.isIsolated || this.networkId == 0)) { if (!(this.isIsolated || this.networkId == 0)) {
if (thing.terminate) { if (thing.terminate) {
DestroyMsg destroyMsg = new(this.networkId, thing); DestroyMsg destroyMsg = new(this.networkId, thing);
// this.Send(this.remoteSite, destroyMsg); this.Send(this.remoteSite, destroyMsg);
this.remoteSite.Send(destroyMsg);
} }
else { else {
// Send to remote site // Send to remote site
// PoseMsg poseMsg = new(thing.owner.networkId, thing); PoseMsg poseMsg = new(thing.owner.networkId, thing);
// this.Send(this.remoteSite, poseMsg); this.Send(this.remoteSite, poseMsg);
// BinaryMsg binaryMsg = new(thing.owner.networkId, thing); BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
// this.Send(this.remoteSite, binaryMsg); this.Send(this.remoteSite, binaryMsg);
this.remoteSite.Send(new PoseMsg(thing.owner.networkId, thing));
this.remoteSite.Send(new BinaryMsg(thing.owner.networkId, thing));
} }
} }
if (thing.terminate) if (thing.terminate)
@ -219,12 +215,10 @@ namespace RoboidControl {
if (thing == null) if (thing == null)
continue; continue;
// PoseMsg poseMsg = new(thing.owner.networkId, thing); PoseMsg poseMsg = new(thing.owner.networkId, thing);
// this.Send(participant, poseMsg); this.Send(participant, poseMsg);
// BinaryMsg binaryMsg = new(thing.owner.networkId, thing); BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
// this.Send(participant, binaryMsg); this.Send(participant, binaryMsg);
participant.Send(new PoseMsg(thing.owner.networkId, thing));
participant.Send(new BinaryMsg(thing.owner.networkId, thing));
} }
} }
@ -236,16 +230,11 @@ namespace RoboidControl {
public void SendThingInfo(Participant owner, Thing thing) { public void SendThingInfo(Participant owner, Thing thing) {
// Console.WriteLine("Send thing info"); // Console.WriteLine("Send thing info");
// this.Send(owner, new ThingMsg(this.networkId, thing)); this.Send(owner, new ThingMsg(this.networkId, thing));
// this.Send(owner, new NameMsg(this.networkId, thing)); this.Send(owner, new NameMsg(this.networkId, thing));
// this.Send(owner, new ModelUrlMsg(this.networkId, thing)); this.Send(owner, new ModelUrlMsg(this.networkId, thing));
// this.Send(owner, new PoseMsg(this.networkId, thing)); this.Send(owner, new PoseMsg(this.networkId, thing));
// this.Send(owner, new BinaryMsg(this.networkId, thing)); this.Send(owner, new BinaryMsg(this.networkId, thing));
owner.Send(new ThingMsg(this.networkId, thing));
owner.Send(new NameMsg(this.networkId, thing));
owner.Send(new ModelUrlMsg(this.networkId, thing));
owner.Send(new PoseMsg(this.networkId, thing));
owner.Send(new BinaryMsg(this.networkId, thing));
} }
public void PublishThingInfo(Thing thing) { public void PublishThingInfo(Thing thing) {
@ -257,16 +246,16 @@ namespace RoboidControl {
this.Publish(new BinaryMsg(this.networkId, thing)); this.Publish(new BinaryMsg(this.networkId, thing));
} }
// public bool Send(Participant owner, IMessage msg) { public bool Send(Participant owner, IMessage msg) {
// int bufferSize = msg.Serialize(ref this.buffer); int bufferSize = msg.Serialize(ref this.buffer);
// if (bufferSize <= 0) if (bufferSize <= 0)
// return true; return true;
// IPEndPoint participantEndpoint = new IPEndPoint(IPAddress.Parse(owner.ipAddress), owner.port); IPEndPoint participantEndpoint = new IPEndPoint(IPAddress.Parse(owner.ipAddress), owner.port);
// // Console.WriteLine($"msg to {participantEndpoint.Address.ToString()} {participantEndpoint.Port}"); // Console.WriteLine($"msg to {participantEndpoint.Address.ToString()} {participantEndpoint.Port}");
// this.udpClient?.Send(this.buffer, bufferSize, participantEndpoint); this.udpClient?.Send(this.buffer, bufferSize, participantEndpoint);
// return true; return true;
// } }
public bool Publish(IMessage msg) { public bool Publish(IMessage msg) {
int bufferSize = msg.Serialize(ref this.buffer); int bufferSize = msg.Serialize(ref this.buffer);
@ -297,7 +286,7 @@ namespace RoboidControl {
string ipAddress = endPoint.Address.ToString(); string ipAddress = endPoint.Address.ToString();
if (ipAddress != this.ipAddress) { if (ipAddress != this.ipAddress) {
Participant remoteParticipant = GetParticipant(ipAddress, endPoint.Port); Participant remoteParticipant = GetParticipant(ipAddress, endPoint.Port);
remoteParticipant ??= AddParticipant(ipAddress, endPoint.Port, this); remoteParticipant ??= AddParticipant(ipAddress, endPoint.Port);
ReceiveData(data, remoteParticipant); ReceiveData(data, remoteParticipant);
} }

39
src/Participants/SiteServer.cs
View File

@ -82,12 +82,10 @@ namespace RoboidControl {
if (participant == null || participant == this) if (participant == null || participant == this)
continue; continue;
// PoseMsg poseMsg = new(thing.owner.networkId, thing); PoseMsg poseMsg = new(thing.owner.networkId, thing);
// this.Send(participant, poseMsg); this.Send(participant, poseMsg);
// BinaryMsg binaryMsg = new(thing.owner.networkId, thing); BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
// this.Send(participant, binaryMsg); this.Send(participant, binaryMsg);
participant.Send(new PoseMsg(thing.owner.networkId, thing));
participant.Send(new BinaryMsg(thing.owner.networkId, thing));
} }
} }
} }
@ -101,23 +99,26 @@ namespace RoboidControl {
base.Process(sender, msg); base.Process(sender, msg);
if (msg.networkId != sender.networkId) { if (msg.networkId != sender.networkId) {
//Console.WriteLine($"{this.name} received New Participant -> {sender.networkId}"); //Console.WriteLine($"{this.name} received New Participant -> {sender.networkId}");
// this.Send(sender, new NetworkIdMsg(sender.networkId)); this.Send(sender, new NetworkIdMsg(sender.networkId));
sender.Send(new NetworkIdMsg(sender.networkId));
UpdateEvent e = new() {
messageId = ParticipantMsg.Id,
participant = sender
};
this.updateQueue.Enqueue(e);
} }
} }
protected override void Process(Participant sender, NetworkIdMsg msg) { } protected override void Process(Participant sender, NetworkIdMsg msg) { }
protected override void Process(Participant sender, ThingMsg msg) { protected override void Process(Participant sender, ThingMsg msg) {
Console.WriteLine($"{this.name}: Process thing [{msg.networkId}/{msg.thingId}] {msg.thingType} {msg.parentId} "); Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}] {msg.thingType} {msg.parentId} ");
Thing thing = sender.Get(msg.thingId); Thing thing = sender.Get(msg.thingId);
thing ??= ProcessNewThing(sender, msg); 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);
if (msg.parentId != 0) { if (msg.parentId != 0) {
thing.parent = sender.Get(msg.parentId); thing.parent = sender.Get(msg.parentId);
@ -130,14 +131,6 @@ 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 #endregion Receive

71
src/Thing.cs
View File

@ -1,6 +1,5 @@
using System; using System;
using System.Collections.Generic; using System.Collections.Generic;
using System.Collections.Concurrent;
using LinearAlgebra; using LinearAlgebra;
namespace RoboidControl { namespace RoboidControl {
@ -26,7 +25,6 @@ namespace RoboidControl {
public const byte ControlledMotor = 0x06; public const byte ControlledMotor = 0x06;
public const byte UncontrolledMotor = 0x07; public const byte UncontrolledMotor = 0x07;
public const byte Servo = 0x08; public const byte Servo = 0x08;
public const byte IncrementalEncoder = 0x19;
// Other // Other
public const byte Roboid = 0x09; public const byte Roboid = 0x09;
public const byte HUmanoid = 0x0A; public const byte HUmanoid = 0x0A;
@ -58,13 +56,8 @@ namespace RoboidControl {
this.type = thingType; this.type = thingType;
if (this.owner != null) if (this.owner != null)
this.owner.Add(this); this.owner.Add(this);
if (invokeEvent) { if (invokeEvent)
Participant.UpdateEvent e = new() { InvokeNewThing(this);
messageId = ThingMsg.id,
thing = this
};
this.owner.updateQueue.Enqueue(e);
}
} }
/// <summary> /// <summary>
@ -138,25 +131,20 @@ namespace RoboidControl {
if (_name != value) { if (_name != value) {
_name = value; _name = value;
nameChanged = true; nameChanged = true;
this.updateQueue.Enqueue(new CoreEvent(NameMsg.Id)); OnNameChanged?.Invoke();
} }
} }
} }
/// <summary>
/// Event which is triggered when the name changes
/// </summary>
public event ChangeHandler OnNameChanged = delegate { };
public bool nameChanged = false; public bool nameChanged = false;
private string _modelUrl = "";
/// <summary> /// <summary>
/// An URL pointing to the location where a model of the thing can be found /// An URL pointing to the location where a model of the thing can be found
/// </summary> /// </summary>
public string modelUrl { public string modelUrl = "";
get => _modelUrl;
set {
if (_modelUrl != value) {
_modelUrl = value;
this.updateQueue.Enqueue(new CoreEvent(ModelUrlMsg.Id));
}
}
}
#if UNITY_5_3_OR_NEWER #if UNITY_5_3_OR_NEWER
/// <summary> /// <summary>
@ -188,7 +176,6 @@ namespace RoboidControl {
value.AddChild(this); value.AddChild(this);
} }
this.hierarchyChanged = true; this.hierarchyChanged = true;
this.updateQueue.Enqueue(new CoreEvent(ThingMsg.id));
} }
} }
@ -283,11 +270,15 @@ namespace RoboidControl {
if (_position != value) { if (_position != value) {
_position = value; _position = value;
positionUpdated = true; positionUpdated = true;
updateQueue.Enqueue(new CoreEvent(PoseMsg.Id)); //OnPositionChanged?.Invoke();
} }
} }
} }
/// <summary> /// <summary>
/// Event triggered when the pose has changed
/// </summary>
public event ChangeHandler OnPoseChanged = delegate { };
/// <summary>
/// Boolean indicating that the thing has an updated position /// Boolean indicating that the thing has an updated position
/// </summary> /// </summary>
public bool positionUpdated = false; public bool positionUpdated = false;
@ -321,11 +312,15 @@ namespace RoboidControl {
if (_linearVelocity != value) { if (_linearVelocity != value) {
_linearVelocity = value; _linearVelocity = value;
linearVelocityUpdated = true; linearVelocityUpdated = true;
updateQueue.Enqueue(new CoreEvent(PoseMsg.Id)); OnLinearVelocityChanged?.Invoke(_linearVelocity);
} }
} }
} }
/// <summary> /// <summary>
/// Event triggered when the linear velocity has changed
/// </summary>
public event SphericalHandler OnLinearVelocityChanged = delegate { };
/// <summary>
/// Boolean indicating the thing has an updated linear velocity /// Boolean indicating the thing has an updated linear velocity
/// </summary> /// </summary>
public bool linearVelocityUpdated = false; public bool linearVelocityUpdated = false;
@ -340,11 +335,15 @@ namespace RoboidControl {
if (_angularVelocity != value) { if (_angularVelocity != value) {
_angularVelocity = value; _angularVelocity = value;
angularVelocityUpdated = true; angularVelocityUpdated = true;
updateQueue.Enqueue(new CoreEvent(PoseMsg.Id)); OnAngularVelocityChanged?.Invoke(_angularVelocity);
} }
} }
} }
/// <summary> /// <summary>
/// Event triggered when the angular velocity has changed
/// </summary>
public event SphericalHandler OnAngularVelocityChanged = delegate { };
/// <summary>
/// Boolean indicating the thing has an updated angular velocity /// Boolean indicating the thing has an updated angular velocity
/// </summary> /// </summary>
public bool angularVelocityUpdated = false; public bool angularVelocityUpdated = false;
@ -379,6 +378,8 @@ namespace RoboidControl {
/// <param name="currentTime">he current clock time in milliseconds; if this is zero, the current time is retrieved automatically</param> /// <param name="currentTime">he current clock time in milliseconds; if this is zero, the current time is retrieved automatically</param>
/// <param name="recurse">When true, this will Update the descendants recursively</param> /// <param name="recurse">When true, this will Update the descendants recursively</param>
public virtual void Update(ulong currentTimeMs, bool recurse = false) { public virtual void Update(ulong currentTimeMs, bool recurse = false) {
if (this.positionUpdated || this.orientationUpdated)
OnPoseChanged?.Invoke();
this.positionUpdated = false; this.positionUpdated = false;
this.orientationUpdated = false; this.orientationUpdated = false;
this.linearVelocityUpdated = false; this.linearVelocityUpdated = false;
@ -396,13 +397,21 @@ namespace RoboidControl {
} }
} }
public class CoreEvent { public delegate void ChangeHandler();
public CoreEvent(int messageId) { public delegate void SphericalHandler(Spherical v);
this.messageId = messageId; public delegate void ThingHandler(Thing t);
}
public int messageId; // see the communication messages /// <summary>
}; /// Event triggered when a new thing has been created
public ConcurrentQueue<CoreEvent> updateQueue = new(); /// </summary>
public static event ThingHandler OnNewThing = delegate { };
/// <summary>
/// Trigger the creation for the given thing
/// </summary>
/// <param name="thing">The created thing</param>
public static void InvokeNewThing(Thing thing) {
OnNewThing?.Invoke(thing);
}
#endregion Update #endregion Update

116
src/Things/DifferentialDrive.cs
View File

@ -17,19 +17,7 @@ namespace RoboidControl {
/// <param name="owner">The owning participant</param> /// <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="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(ParticipantUDP participant, byte thingId = 0, bool invokeEvent = true) : base(participant, Type.DifferentialDrive, thingId, invokeEvent) { }
// Motor leftWheel = new(this) {
// name = "Left Wheel"
// };
// Motor 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> /// <summary>
/// Create a new child differential drive /// Create a new child differential drive
/// </summary> /// </summary>
@ -45,38 +33,31 @@ namespace RoboidControl {
/// These values are used to compute the desired wheel speed from the set /// These values are used to compute the desired wheel speed from the set
/// linear and angular velocity. /// linear and angular velocity.
/// @sa SetLinearVelocity SetAngularVelocity /// @sa SetLinearVelocity SetAngularVelocity
public void SetDriveDimensions(float wheelDiameter, float wheelSeparation = 0) { 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.rpsToMs = wheelDiameter * Angle.pi; this.rpsToMs = wheelDiameter * Angle.pi;
if (wheelSeparation > 0) { float distance = this.wheelSeparation / 2;
wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation; if (this.leftWheel != null)
float distance = wheelSeparation / 2; this.leftWheel.position = new Spherical(distance, Direction.left);
if (this.leftWheel != null) if (this.rightWheel != null)
this.leftWheel.position = new Spherical(distance, Direction.left); this.rightWheel.position = new Spherical(distance, Direction.right);
if (this.rightWheel != null)
this.rightWheel.position = new Spherical(distance, Direction.right);
}
} }
/// @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(Motor leftWheel, Motor rightWheel) { public void SetMotors(Thing leftWheel, Thing rightWheel) {
// float distance = this.wheelSeparation / 2; float distance = this.wheelSeparation / 2;
this.leftWheel = leftWheel; this.leftWheel = leftWheel;
this.leftWheel.parent = this; if (this.leftWheel != null)
// if (this.leftWheel != null) this.leftWheel.position = new Spherical(distance, Direction.left);
// this.leftWheel.position = new Spherical(distance, Direction.left);
this.rightWheel = rightWheel; this.rightWheel = rightWheel;
this.rightWheel.parent= this; if (this.rightWheel != null)
// if (this.rightWheel != null) this.rightWheel.position = new Spherical(distance, Direction.right);
// this.rightWheel.position = new Spherical(distance, Direction.right);
owner.Send(new BinaryMsg(owner.networkId, this));
this.updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id));
} }
/// @brief Directly specify the speeds of the motors /// @brief Directly specify the speeds of the motors
@ -85,26 +66,10 @@ 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.targetSpeed = speedLeft; this.leftWheel.angularVelocity = new Spherical(speedLeft, Direction.left);
//this.leftWheel.angularVelocity = new Spherical(speedLeft, Direction.left); if (this.rightWheel != null)
} this.rightWheel.angularVelocity = new Spherical(speedRight, Direction.right);
if (this.rightWheel != null) {
this.rightWheel.targetSpeed = speedRight;
//this.rightWheel.angularVelocity = new Spherical(speedRight, Direction.right);
}
}
/// @brief Directly specify the speeds of the motors
/// @param speedLeft The speed of the left wheel in degrees per second.
/// Positive moves the robot in the forward direction.
/// @param speedRight The speed of the right wheel in degrees per second.
/// Positive moves the robot in the forward direction.
public void SetWheelAngularVelocity(float angularSpeedLeft, float angularSpeedRight) {
// This only works when the motor is a motor with encoder
if (this.leftWheel is EncoderMotor leftMotor)
leftMotor.targetAngularSpeed = angularSpeedLeft;
if (this.rightWheel is EncoderMotor rightMotor)
rightMotor.targetAngularSpeed = angularSpeedRight;
} }
/// @copydoc RoboidControl::Thing::Update(unsigned long) /// @copydoc RoboidControl::Thing::Update(unsigned long)
@ -119,54 +84,25 @@ namespace RoboidControl {
if (angularVelocity.direction.horizontal < 0) if (angularVelocity.direction.horizontal < 0)
angularSpeed = -angularSpeed; angularSpeed = -angularSpeed;
// This assumes that the left wheel position has Direction.left // wheel separation can be replaced by this.leftwheel.position.distance
// and the right wheel position has Direction.Right... float speedLeft = (linearVelocity + angularSpeed * this.wheelSeparation / 2) / this.wheelRadius * Angle.Rad2Deg;
float speedLeft = (linearVelocity + angularSpeed * this.leftWheel.position.distance) / this.wheelRadius * Angle.Rad2Deg; float speedRight = (linearVelocity - angularSpeed * this.wheelSeparation / 2) / this.wheelRadius * Angle.Rad2Deg;
float speedRight = (linearVelocity - angularSpeed * this.rightWheel.position.distance) / this.wheelRadius * Angle.Rad2Deg;
this.SetWheelVelocity(speedLeft, speedRight); this.SetWheelVelocity(speedLeft, speedRight);
} }
} }
/// @brief The radius of a wheel in meters /// @brief The radius of a wheel in meters
private float _wheelRadius = 0.0f; protected float wheelRadius = 1.0f;
public float wheelRadius { get => _wheelRadius; }
/// @brief The distance between the wheels in meters /// @brief The distance between the wheels in meters
// private float _wheelSeparation = 0.0f; protected float wheelSeparation = 1.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 Motor leftWheel = null; protected Thing leftWheel = null;
/// @brief The right wheel /// @brief The right wheel
public Motor rightWheel = null; protected Thing rightWheel = null;
bool sendBinary = false;
public override byte[] GenerateBinary() {
if (!sendBinary)
return System.Array.Empty<byte>();
byte[] data = new byte[4];
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) as Motor;
byte rightWheelId = data[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 CoreEvent(BinaryMsg.Id));
}
}; };
} // namespace RoboidControl } // namespace RoboidControl

84
src/Things/EncoderMotor.cs
View File

@ -1,84 +0,0 @@
namespace RoboidControl {
/// @brief A motor with speed control
/// It uses a feedback loop from an encoder to regulate the speed
/// The speed is measured in revolutions per second.
class EncoderMotor : Motor {
public EncoderMotor(Thing parent, RelativeEncoder encoder) : base(parent) {
this.encoder = encoder;
}
// Upgrade an existing motor with an encoder
public EncoderMotor(Motor motor, RelativeEncoder encoder) : base(motor.parent) {
this.motor = motor;
this.encoder = encoder;
}
readonly RelativeEncoder encoder;
readonly Motor motor;
public override float targetSpeed {
get {
if (this.motor != null)
return this.motor.targetSpeed;
else
return base.targetSpeed;
}
set {
if (this.motor != null)
this.motor.targetSpeed = value;
else
base.targetSpeed = value;
}
}
private float _targetAngularSpeed; // In degrees per second
public virtual float targetAngularSpeed {
get => _targetAngularSpeed;
set {
if (value != this._targetAngularSpeed) {
this._targetAngularSpeed = value;
updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id));
owner.Send(new BinaryMsg(this));
}
}
}
/// @brief Get the actual speed from the encoder
/// @return The speed in angle per second
public virtual float actualAngularSpee {
get => encoder.angularSpeed;
}
float pidP = 0.1F;
float pidD = 0.0F;
float pidI = 0.0F;
public override void Update(ulong currentTimeMs, bool recurse = false) {
float actualSpeed = this.encoder.angularSpeed;
// Simplified rotation direction, shouldbe improved
// This goes wrong when the target speed is inverted and the motor axcle is still turning in the old direction
if (this.targetAngularSpeed < 0)
actualSpeed = -actualSpeed;
float deltaTime = (currentTimeMs - this.lastUpdateTime) / 1000.0f;
float error = this.targetAngularSpeed - actualSpeed;
float errorChange = (error - lastError) * deltaTime;
float delta = error * pidP + errorChange * pidD;
// Update the motor speed
if (motor != null)
motor.targetSpeed += delta;
else
base.targetSpeed += delta;
this.lastUpdateTime = currentTimeMs;
}
ulong lastUpdateTime = 0;
float lastError = 0;
// enum Direction { Forward = 1, Reverse = -1 };
// Direction rotationDirection;
};
} // namespace RoboidControl

45
src/Things/Motor.cs
View File

@ -1,45 +0,0 @@
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 = 0;
/// <summary>
/// The speed between -1 (full reverse), 0 (stop) and 1 (full forward)
/// </summary>
public virtual float targetSpeed {
get => _targetSpeed;
set {
if (value != _targetSpeed) {
_targetSpeed = Float.Clamp(value, -1, 1);
updateQueue.Enqueue(new CoreEvent(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;
}
}
}

40
src/Things/RelativeEncoder.cs
View File

@ -1,40 +0,0 @@
namespace RoboidControl {
/// @brief An Incremental Encoder measures the rotations of an axle using a rotary
/// sensor. Some encoders are able to detect direction, while others can not.
public class RelativeEncoder : Thing {
/// @brief Creates a sensor which measures distance from pulses
/// @param pulsesPerRevolution The number of pulse edges which make a
/// full rotation
/// @param distancePerRevolution The distance a wheel travels per full
/// rotation
public RelativeEncoder(bool invokeEvent = true) : base(Type.IncrementalEncoder, invokeEvent) { }
protected float _rotationSpeed = 0;
/// @brief Get the rotation speed since the previous call
/// @param currentTimeMs The clock time in milliseconds
/// @return The speed in rotations per second
public virtual float angularSpeed {
get => _rotationSpeed;
set {
if (_rotationSpeed != value) {
_rotationSpeed = value;
this.owner.Send(new BinaryMsg(this));
}
}
}
public override byte[] GenerateBinary() {
byte[] data = new byte[1];
byte ix = 0;
data[ix++] = (byte)(this._rotationSpeed * 127);
return data;
}
public override void ProcessBinary(byte[] data) {
this._rotationSpeed = (float)data[0] / 127;
}
};
} // namespace RoboidControl

4
src/Things/TouchSensor.cs
View File

@ -24,9 +24,7 @@ namespace RoboidControl {
/// <param name="parent">The parent thing</param> /// <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="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 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> /// <summary>
/// Value which is true when the sensor is touching something, false otherwise /// Value which is true when the sensor is touching something, false otherwise