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

Added EncoderMotor support

Browse Source
This commit is contained in:
Pascal Serrarens 2025-05-12 12:41:24 +02:00
parent 71939b9aa5
commit 8c7e7e03f7
11 changed files with 334 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Examples/BB2B/BB2B.cs
View File

@ -12,8 +12,6 @@ namespace RoboidControl {
public BB2B(Participant owner) : base(owner) {
this.name = "BB2B";
this.SetDriveDimensions(0.064f, 0.128f);
// this.wheelRadius = 0.032f;
// this.wheelSeparation = 0.128f;
// Is has a touch sensor at the front left of the roboid
touchLeft = new(this) {

55
Examples/BB2B/BB2B_Encoder.cs Normal file
View File

@ -0,0 +1,55 @@
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);
}
}
}

20
Unity/DifferentialDrive.cs
View File

@ -49,24 +49,26 @@ namespace RoboidControl.Unity {
protected override void HandleBinary() {
Debug.Log("Diff drive handle Binary");
if (coreDrive.wheelRadius <= 0 || coreDrive.wheelSeparation <= 0)
if (coreDrive.wheelRadius <= 0) // || coreDrive.wheelSeparation <= 0)
return;
if (leftWheel == null)
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;
// leftWheelCollider.center = new Vector3(-coreDrive.wheelSeparation / 2, 0, 0);
}
if (rightWheel == null)
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;
// rightWheelCollider.center = new Vector3(coreDrive.wheelSeparation / 2, 0, 0);
}
if (casterWheel == null) {
GameObject gameObj = new("Caster wheel");
@ -76,7 +78,9 @@ namespace RoboidControl.Unity {
}
casterWheel.radius = coreDrive.wheelRadius;
// Put it in the middle of the back
casterWheel.center = new Vector3(0, 0, -coreDrive.wheelSeparation);
// 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);
}
protected override void FixedUpdate() {
@ -86,8 +90,10 @@ namespace RoboidControl.Unity {
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) / coreDrive.wheelSeparation;
float turningSpeed = (leftWheelVelocity - rightWheelVelocity) / wheelSeparation;
// Use smoothing to emulate motor inertia
rb.velocity = 0.9f * rb.velocity + 0.1f * forwardSpeed * transform.forward;

3
Unity/Participant.cs
View File

@ -27,10 +27,12 @@ namespace RoboidControl.Unity {
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;
@ -41,6 +43,7 @@ namespace RoboidControl.Unity {
// // 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;

73
Unity/RelativeEncoder.cs Normal file
View File

@ -0,0 +1,73 @@
#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

5
Unity/Thing.cs
View File

@ -97,15 +97,18 @@ namespace RoboidControl.Unity {
private void HandleUpdateEvent(RoboidControl.Thing.UpdateEvent e) {
switch (e.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;
break;
case ModelUrlMsg.Id:
Debug.Log("{this.id} Handle Model URL");
string extension = core.modelUrl[core.modelUrl.LastIndexOf(".")..];
if (extension == ".jpg" || extension == ".png")
StartCoroutine(LoadJPG());
@ -113,9 +116,11 @@ namespace RoboidControl.Unity {
this.modelUrl = core.modelUrl;
break;
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;
}

1
src/Thing.cs
View File

@ -26,6 +26,7 @@ namespace RoboidControl {
public const byte ControlledMotor = 0x06;
public const byte UncontrolledMotor = 0x07;
public const byte Servo = 0x08;
public const byte IncrementalEncoder = 0x19;
// Other
public const byte Roboid = 0x09;
public const byte HUmanoid = 0x0A;

72
src/Things/DifferentialDrive.cs
View File

@ -18,16 +18,16 @@ namespace RoboidControl {
/// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
/// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
public DifferentialDrive(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.DifferentialDrive, thingId, invokeEvent) {
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));
// 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>
@ -45,31 +45,38 @@ namespace RoboidControl {
/// These values are used to compute the desired wheel speed from the set
/// linear and angular velocity.
/// @sa SetLinearVelocity SetAngularVelocity
public void SetDriveDimensions(float wheelDiameter, float wheelSeparation) {
public void SetDriveDimensions(float wheelDiameter, float wheelSeparation = 0) {
this._wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
this._wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
this.rpsToMs = wheelDiameter * Angle.pi;
float distance = this.wheelSeparation / 2;
if (wheelSeparation > 0) {
wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
float distance = wheelSeparation / 2;
if (this.leftWheel != null)
this.leftWheel.position = new Spherical(distance, Direction.left);
if (this.rightWheel != null)
this.rightWheel.position = new Spherical(distance, Direction.right);
}
}
/// @brief Congures the motors for the wheels
/// @param leftWheel The motor for the left wheel
/// @param rightWheel The motor for the right wheel
public void SetMotors(Motor leftWheel, Motor rightWheel) {
float distance = this.wheelSeparation / 2;
// float distance = this.wheelSeparation / 2;
this.leftWheel = leftWheel;
if (this.leftWheel != null)
this.leftWheel.position = new Spherical(distance, Direction.left);
this.leftWheel.parent = this;
// if (this.leftWheel != null)
// this.leftWheel.position = new Spherical(distance, Direction.left);
this.rightWheel = rightWheel;
if (this.rightWheel != null)
this.rightWheel.position = new Spherical(distance, Direction.right);
this.rightWheel.parent= this;
// if (this.rightWheel != null)
// this.rightWheel.position = new Spherical(distance, Direction.right);
owner.Send(new BinaryMsg(owner.networkId, this));
this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id));
}
/// @brief Directly specify the speeds of the motors
@ -87,6 +94,18 @@ namespace RoboidControl {
//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)
public override void Update(ulong currentMs, bool recursive = true) {
@ -100,9 +119,10 @@ namespace RoboidControl {
if (angularVelocity.direction.horizontal < 0)
angularSpeed = -angularSpeed;
// wheel separation can be replaced by this.leftwheel.position.distance
float speedLeft = (linearVelocity + angularSpeed * this.wheelSeparation / 2) / this.wheelRadius * Angle.Rad2Deg;
float speedRight = (linearVelocity - angularSpeed * this.wheelSeparation / 2) / this.wheelRadius * Angle.Rad2Deg;
// This assumes that the left wheel position has Direction.left
// and the right wheel position has Direction.Right...
float speedLeft = (linearVelocity + angularSpeed * this.leftWheel.position.distance) / this.wheelRadius * Angle.Rad2Deg;
float speedRight = (linearVelocity - angularSpeed * this.rightWheel.position.distance) / this.wheelRadius * Angle.Rad2Deg;
this.SetWheelVelocity(speedLeft, speedRight);
}
}
@ -111,8 +131,8 @@ namespace RoboidControl {
private float _wheelRadius = 0.0f;
public float wheelRadius { get => _wheelRadius; }
/// @brief The distance between the wheels in meters
private float _wheelSeparation = 0.0f;
public float wheelSeparation { get => _wheelSeparation; }
// private float _wheelSeparation = 0.0f;
// public float wheelSeparation { get => _wheelSeparation; }
/// @brief Convert revolutions per second to meters per second
protected float rpsToMs = 1.0f;
@ -127,12 +147,12 @@ namespace RoboidControl {
if (!sendBinary)
return System.Array.Empty<byte>();
byte[] data = new byte[6];
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);
//LowLevelMessages.SendFloat16(data, ref ix, wheelSeparation);
sendBinary = false;
return data;
}
@ -144,7 +164,7 @@ namespace RoboidControl {
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._wheelSeparation = LowLevelMessages.ReceiveFloat16(data, ref ix);
this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id));
}
};

84
src/Things/EncoderMotor.cs Normal file
View File

@ -0,0 +1,84 @@
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 UpdateEvent(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

4
src/Things/Motor.cs
View File

@ -16,11 +16,11 @@ namespace RoboidControl {
protected float currentTargetSpeed = 0;
private float _targetSpeed;
private float _targetSpeed = 0;
/// <summary>
/// The speed between -1 (full reverse), 0 (stop) and 1 (full forward)
/// </summary>
public float targetSpeed {
public virtual float targetSpeed {
get => _targetSpeed;
set {
if (value != _targetSpeed) {

40
src/Things/RelativeEncoder.cs Normal file
View File

@ -0,0 +1,40 @@
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