Added EncoderMotor support

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) {

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);
+        }
+    }
+
+}

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<SphereCollider>();
+                leftWheelCollider.radius = coreDrive.wheelRadius;
+                // leftWheelCollider.center = new Vector3(-coreDrive.wheelSeparation / 2, 0, 0);
+            }
 
-            leftWheel.core ??= coreDrive.leftWheel;
+            if (rightWheel == null && coreDrive.rightWheel != null)
-            SphereCollider leftWheelCollider = leftWheel.GetComponent<SphereCollider>();
-            leftWheelCollider.radius = coreDrive.wheelRadius;
-            // leftWheelCollider.center = new Vector3(-coreDrive.wheelSeparation / 2, 0, 0);
-
-            if (rightWheel == null)
                 rightWheel = Wheel.Create(this.transform, coreDrive.rightWheel.id);
-
+            if (rightWheel != null) {
-            rightWheel.core ??= coreDrive.rightWheel;
+                rightWheel.core ??= coreDrive.rightWheel;
-            SphereCollider rightWheelCollider = rightWheel.GetComponent<SphereCollider>();
+                SphereCollider rightWheelCollider = rightWheel.GetComponent<SphereCollider>();
-            rightWheelCollider.radius = coreDrive.wheelRadius;
+                rightWheelCollider.radius = coreDrive.wheelRadius;
-            // rightWheelCollider.center = new Vector3(coreDrive.wheelSeparation / 2, 0, 0);
+                // 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;

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;

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 {
+
+        /// <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

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;
             }

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;

src/Things/DifferentialDrive.cs

@@ -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) {
+            // Motor leftWheel = new(this) {
-                name = "Left Wheel"
+            //     name = "Left Wheel"
-            };
+            // };
-            Motor rightWheel = new(this) {
+            // Motor rightWheel = new(this) {
-                name = "Right Wheel"
+            //     name = "Right Wheel"
-            };
+            // };
-            SetMotors(leftWheel, rightWheel);
+            // SetMotors(leftWheel, rightWheel);
-            sendBinary = true;
+            // sendBinary = true;
-            owner.Send(new BinaryMsg(owner.networkId, this));
+            // owner.Send(new BinaryMsg(owner.networkId, this));
-            this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id));
+            // 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) {
-            if (this.leftWheel != null)
+                wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
-                this.leftWheel.position = new Spherical(distance, Direction.left);
+                float distance = wheelSeparation / 2;
-            if (this.rightWheel != null)
+                if (this.leftWheel != null)
-                this.rightWheel.position = new Spherical(distance, Direction.right);
+                    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.parent = this;
-                this.leftWheel.position = new Spherical(distance, Direction.left);
+            // if (this.leftWheel != null)
+            //     this.leftWheel.position = new Spherical(distance, Direction.left);
 
             this.rightWheel = rightWheel;
-            if (this.rightWheel != null)
+            this.rightWheel.parent= this;
-                this.rightWheel.position = new Spherical(distance, Direction.right);
+            // 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
+                // This assumes that the left wheel position has Direction.left
-                float speedLeft = (linearVelocity + angularSpeed * this.wheelSeparation / 2) / this.wheelRadius * Angle.Rad2Deg;
+                // and the right wheel position has Direction.Right...
-                float speedRight = (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.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;
+        // private float _wheelSeparation = 0.0f;
-        public float wheelSeparation { get => _wheelSeparation; }
+        // 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));
         }
     };

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

src/Things/Motor.cs

@@ -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) {

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