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