diff --git a/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/AngularServoBase.cs b/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/AngularServoBase.cs new file mode 100644 index 0000000000..be9571b1b3 --- /dev/null +++ b/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 + { + /// + /// Returns the current angle. + /// + public Angle? Angle { get; protected set; } + + /// + /// Instantiates a new Servo on the specified PWM Pin with the specified config. + /// + /// + /// + public AngularServoBase(IPwmPort pwm, ServoConfig config) + : base(pwm, config) + { + } + + /// + /// Rotates the servo to a given angle. + /// + /// The angle to rotate to. + /// When true the PWM will stop after motion is complete. + 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 + } + } +} \ No newline at end of file diff --git a/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/ContinuousRotationServoBase.cs b/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/ContinuousRotationServoBase.cs index f543a9827f..fe3bfc397b 100644 --- a/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/ContinuousRotationServoBase.cs +++ b/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/ContinuousRotationServoBase.cs @@ -6,36 +6,17 @@ namespace Meadow.Foundation.Servos /// /// Base class implementation for servos that can rotate continuously. /// - public abstract class ContinuousRotationServoBase : IContinuousRotationServo + public abstract class ContinuousRotationServoBase : ServoBase, IContinuousRotationServo { - protected IPwmPort _pwm = null; - - /// - /// Gets the ServoConfig that describes this servo. - /// - public ServoConfig Config - { - get { return _config; } - } - protected ServoConfig _config = null; - /// /// Gets the current rotation direction. /// - public RotationDirection CurrentDirection - { - get { return _currentDirection; } - } - protected RotationDirection _currentDirection = RotationDirection.None; + public RotationDirection CurrentDirection { get; protected set; } = RotationDirection.None; /// /// Gets the current rotation speed. /// - public float CurrentSpeed - { - get { return _currentSpeed; } - } - protected float _currentSpeed = -1; + public float CurrentSpeed { get; protected set; } = -1; /// /// Instantiates a new continuous rotation servo on the specified pin, with the specified configuration. @@ -43,15 +24,8 @@ public float CurrentSpeed /// /// 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; - } /// @@ -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; } /// /// Stops rotation of the servo. /// - public void Stop() + public override void Stop() { - _pwm.Stop(); - _currentDirection = RotationDirection.None; - _currentSpeed = 0.0f; + base.Stop(); + CurrentDirection = RotationDirection.None; + CurrentSpeed = 0.0f; } /// @@ -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 } } diff --git a/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/IAngularServo.cs b/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/IAngularServo.cs new file mode 100644 index 0000000000..b74d3a80df --- /dev/null +++ b/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; } + } +} \ No newline at end of file diff --git a/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/IContinuousRotationServo.cs b/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/IContinuousRotationServo.cs index abe4400e09..efa870ae53 100644 --- a/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/IContinuousRotationServo.cs +++ b/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; } diff --git a/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/IServo.cs b/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/IServo.cs index e9b9cbcd14..0d3e47644a 100644 --- a/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/IServo.cs +++ b/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; } } } \ No newline at end of file diff --git a/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/NamedServoConfigs.cs b/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/NamedServoConfigs.cs index 0c9a5ad0d3..69cf29b4e7 100644 --- a/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/NamedServoConfigs.cs +++ b/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 /// 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); + + /// + /// Represents the MG996R 180 degree servo models. Angle: 0-180, Pulse: 500 - 2,200 + /// 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); } } \ No newline at end of file diff --git a/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/ServoCore.cs b/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/Servo.cs similarity index 87% rename from Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/ServoCore.cs rename to Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/Servo.cs index 64fa937227..59a05634ea 100644 --- a/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/ServoCore.cs +++ b/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) { } diff --git a/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/ServoBase.cs b/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/ServoBase.cs index 543b7ebebd..bca09100ac 100644 --- a/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/ServoBase.cs +++ b/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; - /// - /// Gets the ServoConfig that describes this servo. + /// Gets the PWM port used to drive the Servo /// - public ServoConfig Config - { - get { return _config; } - } protected ServoConfig _config = null; + protected IPwmPort PwmPort { get; } /// - /// Returns the current angle. Returns -1 if the angle is unknown. + /// Gets the ServoConfig that describes this servo. /// - public Angle? Angle { get; protected set; } + public ServoConfig Config { get; protected set; } - /// - /// Instantiates a new Servo on the specified PWM Pin with the specified config. - /// - /// - /// - 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; - /// - /// Rotates the servo to a given angle. - /// - /// The angle to rotate to. - 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; - } - - /// - /// Stops the signal that controls the servo angle. For many servos, this will - /// return the servo to its nuetral position (usually 0º). - /// - 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(); } /// @@ -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); } } } \ No newline at end of file diff --git a/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/ServoConfig.cs b/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/ServoConfig.cs index 1c424cdd79..c4fcb1ee8c 100644 --- a/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/ServoConfig.cs +++ b/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) - //{ - //} - /// /// ///