Merge pull request #250 from WildernessLabs/feature/servos

Feature/servos

Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/AngularServoBase.cs

@@ -0,0 +1,75 @@
+using Meadow.Hardware;
+using Meadow.Units;
+using System;
+using System.Threading.Tasks;
+
+namespace Meadow.Foundation.Servos
+{
+    public abstract class AngularServoBase : ServoBase, IAngularServo
+    {
+        /// <summary>
+        /// Returns the current angle.
+        /// </summary>
+        public Angle? Angle { get; protected set; }
+
+        /// <summary>
+        /// Instantiates a new Servo on the specified PWM Pin with the specified config.
+        /// </summary>
+        /// <param name="pwm"></param>
+        /// <param name="config"></param>
+        public AngularServoBase(IPwmPort pwm, ServoConfig config)
+            : base(pwm, config)
+        {
+        }
+
+        /// <summary>
+        /// Rotates the servo to a given angle.
+        /// </summary>
+        /// <param name="angle">The angle to rotate to.</param>
+        /// <param name="stopAfterMotion">When <b>true</b> the PWM will stop after motion is complete.</param>
+        public async Task RotateTo(Angle angle, bool stopAfterMotion = false)
+        {
+            if (!PwmPort.State)
+            {
+                PwmPort.Start();
+            }
+
+            // angle check
+            if (angle < Config.MinimumAngle || angle > Config.MaximumAngle)
+            {
+                throw new ArgumentOutOfRangeException(nameof(angle), "Angle must be within servo configuration tolerance.");
+            }
+
+            // calculate the appropriate pulse duration for the angle
+            float pulseDuration = CalculatePulseDuration(angle);
+
+            // send our pulse to the servo to make it move
+            SendCommandPulse(pulseDuration);
+
+            // wait for completion
+            var rotationRequired = Math.Abs((Angle.HasValue ? Angle.Value.Degrees : 360) - angle.Degrees);
+            var delay = (int)(8 * rotationRequired); // estimating 8ms / degree
+            Console.WriteLine($"Start: {Angle?.Degrees??-1} End:={angle.Degrees}");
+            Console.WriteLine($"degrees={rotationRequired}  Delay={delay}");
+            await Task.Delay(delay);
+
+            // update the state
+            Angle = angle;
+
+            if (stopAfterMotion)
+            {
+                Stop();
+            }
+        }
+
+        protected float CalculatePulseDuration(Angle angle)
+        {
+            // offset + (angle percent * duration length)
+            return Config.MinimumPulseDuration + (float)((angle.Degrees / Config.MaximumAngle.Degrees) * (Config.MaximumPulseDuration - Config.MinimumPulseDuration));
+            // sample calcs:
+            // 0 degrees time = 1000 + ( (0 / 180) * 1000 ) = 1,000 microseconds
+            // 90 degrees time = 1000 + ( (90 / 180) * 1000 ) = 1,500 microseconds
+            // 180 degrees time = 1000 + ( (180 / 180) * 1000 ) = 2,000 microseconds
+        }
+    }
+}

Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/ContinuousRotationServoBase.cs

@@ -6,52 +6,26 @@ namespace Meadow.Foundation.Servos
     /// <summary>
     /// Base class implementation for servos that can rotate continuously.
     /// </summary>
-    public abstract class ContinuousRotationServoBase : IContinuousRotationServo
+    public abstract class ContinuousRotationServoBase : ServoBase, IContinuousRotationServo
     {
-        protected IPwmPort _pwm = null;
-
-        /// <summary>
-        /// Gets the ServoConfig that describes this servo.
-        /// </summary>
-        public ServoConfig Config
-        {
-            get { return _config; }
-        }
-        protected ServoConfig _config = null;
-
         /// <summary>
         /// Gets the current rotation direction.
         /// </summary>
-        public RotationDirection CurrentDirection
-        {
-            get { return _currentDirection; }
-        }
-        protected RotationDirection _currentDirection = RotationDirection.None;
+        public RotationDirection CurrentDirection { get; protected set; } = RotationDirection.None;
 
         /// <summary>
         /// Gets the current rotation speed.
         /// </summary>
-        public float CurrentSpeed
-        {
-            get { return _currentSpeed; }
-        }
-        protected float _currentSpeed = -1;
+        public float CurrentSpeed { get; protected set; } = -1;
 
         /// <summary>
         /// Instantiates a new continuous rotation servo on the specified pin, with the specified configuration.
         /// </summary>
         /// <param name="pin"></param>
         /// <param name="config"></param>
         public ContinuousRotationServoBase(IPwmPort pwm, ServoConfig config)
+            : base(pwm, config)
         {
-            _config = config;
-            // OLD
-			//_pwm = new PWM(pin, config.Frequency, 0, false);
-            // NEW
-            _pwm = pwm;
-            _pwm.Frequency = config.Frequency;
-            _pwm.DutyCycle = 0;
-
         }
 
         /// <summary>
@@ -68,24 +42,23 @@ public void Rotate(RotationDirection direction, float speed)
 
             // calculate the appropriate pulse duration for the speed and direction
             float pulseDuration = CalculatePulseDuration(direction, speed);
-            //Console.WriteLine("Pulse Duration: " + pulseDuration.ToString());
 
             // send our pulse to the servo to make it move
             SendCommandPulse(pulseDuration);
 
             // update state
-            _currentDirection = direction;
-            _currentSpeed = speed;
+            CurrentDirection = direction;
+            CurrentSpeed = speed;
         }
 
         /// <summary>
         /// Stops rotation of the servo.
         /// </summary>
-        public void Stop()
+        public override void Stop()
         {
-            _pwm.Stop();
-            _currentDirection = RotationDirection.None;
-            _currentSpeed = 0.0f;
+            base.Stop();
+            CurrentDirection = RotationDirection.None;
+            CurrentSpeed = 0.0f;
         }
 
         /// <summary>
@@ -100,9 +73,9 @@ public void Stop()
         protected float CalculatePulseDuration(RotationDirection direction, float speed)
         {
             // calculate the midpoint/neutral/stop
-            int midpointPulseDuration = _config.MinimumPulseDuration + ((_config.MaximumPulseDuration - _config.MinimumPulseDuration) / 2);
+            int midpointPulseDuration = Config.MinimumPulseDuration + ((Config.MaximumPulseDuration - Config.MinimumPulseDuration) / 2);
             // the delta is how fast; speed * (max - midpoint)
-            int midPointPulseDelta = (int)(speed * (float)(_config.MaximumPulseDuration - midpointPulseDuration));
+            int midPointPulseDelta = (int)(speed * (float)(Config.MaximumPulseDuration - midpointPulseDuration));
 
             // calculate the pulse direction as less or more than midpoint
             int pulseDuration = midpointPulseDuration;
@@ -118,17 +91,10 @@ protected float CalculatePulseDuration(RotationDirection direction, float speed)
             return pulseDuration;
         }
 
-        protected float CalculateDutyCycle(float pulseDuration)
-        {
-            // the pulse duration is dependent on the frequency we're driving the servo at
-            return pulseDuration / ((1.0f / (float)_config.Frequency) * 1000000f);
-        }
-
-        protected void SendCommandPulse(float pulseDuration)
+        protected override void SendCommandPulse(float pulseDuration)
         {
-            //Console.WriteLine("Sending Command Pulse");
-            _pwm.DutyCycle = CalculateDutyCycle(pulseDuration);
-            _pwm.Start(); // servo expects to run continuously
+            base.SendCommandPulse(pulseDuration);
+            PwmPort.Start(); // servo expects to run continuously
         }
 
     }

Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/IAngularServo.cs

@@ -0,0 +1,12 @@
+using System.Threading.Tasks;
+using Meadow.Units;
+
+namespace Meadow.Foundation.Servos
+{
+    public interface IAngularServo : IServo
+    {
+        Task RotateTo(Angle angle, bool stopAfterMotion = false);
+
+        Angle? Angle { get; }
+    }
+}

Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/IContinuousRotationServo.cs

@@ -2,10 +2,8 @@
 
 namespace Meadow.Foundation.Servos
 {
-    public interface IContinuousRotationServo
+    public interface IContinuousRotationServo : IServo
     {
-        ServoConfig Config { get; }
-
         RotationDirection CurrentDirection { get; }
 
         float CurrentSpeed { get; }

Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/IServo.cs

@@ -1,14 +1,7 @@
-using System;
-using Meadow.Units;
-
 namespace Meadow.Foundation.Servos
 {
     public interface IServo
     {
         ServoConfig Config { get; }
-
-        void RotateTo(Angle angle);
-
-        Angle? Angle { get; }
     }
 }

Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/NamedServoConfigs.cs

@@ -45,5 +45,10 @@ public static class NamedServoConfigs
         /// Represents the SG90 180 degree servo models. Angle: 0-180, Pulse: 500 - 2,200
         /// </summary>0
         public static ServoConfig SG90 = new ServoConfig(minimumAngle: new Angle(0, AU.Degrees), maximumAngle: new Angle(180, AU.Degrees), minimumPulseDuration: 500, maximumPulseDuration: 2350, frequency: 50);
+
+        /// <summary>
+        /// Represents the MG996R 180 degree servo models. Angle: 0-180, Pulse: 500 - 2,200
+        /// </summary>0
+        public static ServoConfig MG996R = new ServoConfig(minimumAngle: new Angle(0, AU.Degrees), maximumAngle: new Angle(180, AU.Degrees), minimumPulseDuration: 500, maximumPulseDuration: 2350, frequency: 50);
     }
 }

Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/ServoCore.cs → Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/Servo.cs

@@ -2,7 +2,7 @@
 
 namespace Meadow.Foundation.Servos
 {
-    public class Servo : ServoBase
+    public class Servo : AngularServoBase
     {
         public Servo(IPwmOutputController device, IPin pwm, ServoConfig config) :
             this(device.CreatePwmPort(pwm), config) { }

Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/ServoBase.cs

@@ -1,81 +1,31 @@
 using Meadow.Hardware;
-using Meadow.Units;
-using System;
 
 namespace Meadow.Foundation.Servos
 {
     public abstract class ServoBase : IServo
     {
-        protected IPwmPort _pwm = null;
-
         /// <summary>
-        /// Gets the ServoConfig that describes this servo.
+        /// Gets the PWM port used to drive the Servo
         /// </summary>
-        public ServoConfig Config
-        {
-            get { return _config; }
-        } protected ServoConfig _config = null;
+        protected IPwmPort PwmPort { get; }
 
         /// <summary>
-        /// Returns the current angle. Returns -1 if the angle is unknown.
+        /// Gets the ServoConfig that describes this servo.
         /// </summary>
-        public Angle? Angle { get; protected set; }
+        public ServoConfig Config { get; protected set; }
 
-        /// <summary>
-        /// Instantiates a new Servo on the specified PWM Pin with the specified config.
-        /// </summary>
-        /// <param name="pwm"></param>
-        /// <param name="config"></param>
-        public ServoBase(IPwmPort pwm, ServoConfig config)
+        protected ServoBase(IPwmPort pwm, ServoConfig config)
         {
-            _config = config;
-
-            _pwm = pwm;
-            _pwm.Frequency = config.Frequency;
-            _pwm.DutyCycle = 0;
-            _pwm.Start();
-        }
+            Config = config;
 
-        /// <summary>
-        /// Rotates the servo to a given angle.
-        /// </summary>
-        /// <param name="angle">The angle to rotate to.</param>
-        public void RotateTo(Angle angle)
-        {
-            // angle check
-            if (angle < _config.MinimumAngle || angle > _config.MaximumAngle) {
-                throw new ArgumentOutOfRangeException(nameof(angle), "Angle must be within servo configuration tolerance.");
-            }
-
-            // calculate the appropriate pulse duration for the angle
-            float pulseDuration = CalculatePulseDuration(angle);
-            //Console.WriteLine("Pulse Duration: " + pulseDuration.ToString());
-
-            // send our pulse to the servo to make it move
-            SendCommandPulse(pulseDuration);
-
-            // update the state
-            Angle = angle;
-        }
-
-        /// <summary>
-        /// Stops the signal that controls the servo angle. For many servos, this will 
-        /// return the servo to its nuetral position (usually 0º).
-        /// </summary>
-        public void Stop()
-        {
-            _pwm.Stop();
-            Angle = null;
+            PwmPort = pwm;
+            PwmPort.Frequency = config.Frequency;
+            PwmPort.DutyCycle = 0;
         }
 
-        protected float CalculatePulseDuration(Angle angle)
+        public virtual void Stop()
         {
-            // offset + (angle percent * duration length)
-            return _config.MinimumPulseDuration + (float)((angle.Degrees / _config.MaximumAngle.Degrees) * (_config.MaximumPulseDuration - _config.MinimumPulseDuration));
-            // sample calcs:
-            // 0 degrees time = 1000 + ( (0 / 180) * 1000 ) = 1,000 microseconds
-            // 90 degrees time = 1000 + ( (90 / 180) * 1000 ) = 1,500 microseconds
-            // 180 degrees time = 1000 + ( (180 / 180) * 1000 ) = 2,000 microseconds
+            PwmPort.Stop();
         }
 
         /// <summary>
@@ -86,13 +36,12 @@ protected float CalculatePulseDuration(Angle angle)
         protected float CalculateDutyCycle(float pulseDuration)
         {
             // the pulse duration is dependent on the frequency we're driving the servo at
-            return pulseDuration / ((1.0f / (float)_config.Frequency) * 1000000f);
+            return pulseDuration / ((1.0f / (float)Config.Frequency) * 1000000f);
         }
 
-        protected void SendCommandPulse(float pulseDuration)
+        protected virtual void SendCommandPulse(float pulseDuration)
         {
-            //Console.WriteLine($"Sending Command Pulse, duration {pulseDuration}, dutycycle: {CalculateDutyCycle(pulseDuration)}");
-            _pwm.DutyCycle = CalculateDutyCycle(pulseDuration);
+            PwmPort.DutyCycle = CalculateDutyCycle(pulseDuration);
         }
     }
 }

Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/ServoConfig.cs

@@ -12,11 +12,6 @@ public class ServoConfig
 
         public int Frequency { get; private set; } // almost always 50hz
 
-        //public ServoConfig(int? minimumAngle = 0, int maximumAngle = 180, int minimumPulseDuration = 1000, int maximumPulseDuration = 2000, int frequency = 50)
-        //    : this(new Angle(minimumAngle?? 0), new Angle(maximumAngle), minimumPulseDuration, maximumPulseDuration, frequency)
-        //{
-        //}
-
         /// <summary>
         /// 
         /// </summary>