diff --git a/Examples/BB2B/BB2B.cs b/Examples/BB2B/BB2B.cs index bc7aa66..815fdb1 100644 --- a/Examples/BB2B/BB2B.cs +++ b/Examples/BB2B/BB2B.cs @@ -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) { diff --git a/Examples/BB2B/BB2B_Encoder.cs b/Examples/BB2B/BB2B_Encoder.cs new file mode 100644 index 0000000..f2b91df --- /dev/null +++ b/Examples/BB2B/BB2B_Encoder.cs @@ -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); + } + } + +} diff --git a/Unity/DifferentialDrive.cs b/Unity/DifferentialDrive.cs index fe74eb3..91c0883 100644 --- a/Unity/DifferentialDrive.cs +++ b/Unity/DifferentialDrive.cs @@ -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(); + leftWheelCollider.radius = coreDrive.wheelRadius; + // leftWheelCollider.center = new Vector3(-coreDrive.wheelSeparation / 2, 0, 0); + } - leftWheel.core ??= coreDrive.leftWheel; - SphereCollider leftWheelCollider = leftWheel.GetComponent(); - 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); - - rightWheel.core ??= coreDrive.rightWheel; - SphereCollider rightWheelCollider = rightWheel.GetComponent(); - rightWheelCollider.radius = coreDrive.wheelRadius; - // rightWheelCollider.center = new Vector3(coreDrive.wheelSeparation / 2, 0, 0); + if (rightWheel != null) { + rightWheel.core ??= coreDrive.rightWheel; + SphereCollider rightWheelCollider = rightWheel.GetComponent(); + 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; diff --git a/Unity/Participant.cs b/Unity/Participant.cs index 9069aab..62834a8 100644 --- a/Unity/Participant.cs +++ b/Unity/Participant.cs @@ -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; diff --git a/Unity/RelativeEncoder.cs b/Unity/RelativeEncoder.cs new file mode 100644 index 0000000..521878c --- /dev/null +++ b/Unity/RelativeEncoder.cs @@ -0,0 +1,73 @@ +#if UNITY_5_3_OR_NEWER +using System.Collections; +using UnityEngine; +using LinearAlgebra; + +namespace RoboidControl.Unity { + public class RelativeEncoder : Thing { + + /// + /// Create the Unity representation + /// + /// The core motor + /// The Unity representation of a motor + 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(); + if (component != null) + component.core = core; + return component; + } + else { + // Fallback implementation + GameObject gameObj = new(core.name); + RelativeEncoder component = gameObj.AddComponent(); + component.Init(core); + + Rigidbody rb = gameObj.AddComponent(); + rb.isKinematic = true; + return component; + } + } + RoboidControl.RelativeEncoder coreEncoder => this.core as RoboidControl.RelativeEncoder; + + /// + /// The rotation speed in degrees per second + /// + 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 \ No newline at end of file diff --git a/Unity/Thing.cs b/Unity/Thing.cs index 423a4b4..51ae580 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -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; } diff --git a/src/Thing.cs b/src/Thing.cs index cee404f..da6ce39 100644 --- a/src/Thing.cs +++ b/src/Thing.cs @@ -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; diff --git a/src/Things/DifferentialDrive.cs b/src/Things/DifferentialDrive.cs index 517b108..97a35c4 100644 --- a/src/Things/DifferentialDrive.cs +++ b/src/Things/DifferentialDrive.cs @@ -18,16 +18,16 @@ namespace RoboidControl { /// The ID of the thing, leave out or set to zero to generate an ID /// Invoke a OnNewThing event when the thing has been created 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)); } /// @@ -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 (this.leftWheel != null) - this.leftWheel.position = new Spherical(distance, Direction.left); - if (this.rightWheel != null) - this.rightWheel.position = new Spherical(distance, Direction.right); + 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[] 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)); } }; diff --git a/src/Things/EncoderMotor.cs b/src/Things/EncoderMotor.cs new file mode 100644 index 0000000..a31a31a --- /dev/null +++ b/src/Things/EncoderMotor.cs @@ -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 diff --git a/src/Things/Motor.cs b/src/Things/Motor.cs index 9b1d6c7..eaf6081 100644 --- a/src/Things/Motor.cs +++ b/src/Things/Motor.cs @@ -16,11 +16,11 @@ namespace RoboidControl { protected float currentTargetSpeed = 0; - private float _targetSpeed; + private float _targetSpeed = 0; /// /// The speed between -1 (full reverse), 0 (stop) and 1 (full forward) /// - public float targetSpeed { + public virtual float targetSpeed { get => _targetSpeed; set { if (value != _targetSpeed) { diff --git a/src/Things/RelativeEncoder.cs b/src/Things/RelativeEncoder.cs new file mode 100644 index 0000000..5183f91 --- /dev/null +++ b/src/Things/RelativeEncoder.cs @@ -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