From c8fdc476e41c94b096853bc82fd419c5b87ba203 Mon Sep 17 00:00:00 2001
From: ctacke <ctacke@opennetcf.com>
Date: Wed, 5 Jan 2022 10:56:44 -0600
Subject: [PATCH] more servo work

---
 .../Servos.ServoCore/AngularServoBase.cs      | 26 +++++++++++--------
 .../Driver/Servos.ServoCore/IAngularServo.cs  | 12 +++++++++
 .../Driver/Servos.ServoCore/IServo.cs         | 10 -------
 .../Driver/Servos.ServoCore/ServoBase.cs      |  7 -----
 4 files changed, 27 insertions(+), 28 deletions(-)
 create mode 100644 Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/IAngularServo.cs

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
index 1488bc99db..be9571b1b3 100644
--- a/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/AngularServoBase.cs
+++ b/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/AngularServoBase.cs
@@ -1,13 +1,14 @@
 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. Returns -1 if the angle is unknown.
+        /// Returns the current angle.
         /// </summary>
         public Angle? Angle { get; protected set; }
 
@@ -26,7 +27,7 @@ public AngularServoBase(IPwmPort pwm, ServoConfig config)
         /// </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 void RotateTo(Angle angle, bool stopAfterMotion = false)
+        public async Task RotateTo(Angle angle, bool stopAfterMotion = false)
         {
             if (!PwmPort.State)
             {
@@ -41,21 +42,24 @@ public void RotateTo(Angle angle, bool stopAfterMotion = false)
 
             // 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;
-        }
 
-        /// <summary>
-        /// Stops the signal that controls the servo angle.
-        /// </summary>
-        public override void Stop()
-        {
-            base.Stop();
-            Angle = null;
+            if (stopAfterMotion)
+            {
+                Stop();
+            }
         }
 
         protected float CalculatePulseDuration(Angle angle)
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/IServo.cs b/Source/Meadow.Foundation.Peripherals/Servos.ServoCore/Driver/Servos.ServoCore/IServo.cs
index 1d29210a30..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,15 +1,5 @@
-using System;
-using Meadow.Units;
-
 namespace Meadow.Foundation.Servos
 {
-    public interface IAngularServo : IServo
-    {
-        void RotateTo(Angle angle, bool stopAfterMotion = false);
-
-        Angle? Angle { get; }
-    }
-
     public interface IServo
     {
         ServoConfig Config { get; }
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 95d952b4ee..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,6 +1,4 @@
 using Meadow.Hardware;
-using Meadow.Units;
-using System;
 
 namespace Meadow.Foundation.Servos
 {
@@ -45,10 +43,5 @@ protected virtual void SendCommandPulse(float pulseDuration)
         {
             PwmPort.DutyCycle = CalculateDutyCycle(pulseDuration);
         }
-
-        public void RotateTo(Angle angle, bool stopAfterMotion = false)
-        {
-            throw new NotImplementedException();
-        }
     }
 }
\ No newline at end of file