ServoPIDControl: Record data from Ardunio into time series. Discard old data.

Author: Marcus Sonestedt

ServoPIDControl/ArduinoCom.cs

@@ -153,6 +153,8 @@ private void LineReceived(string line)
                     servo.Output = float.Parse(parts[3]);
                     servo.Integrator = float.Parse(parts[4]);
                     servo.DFiltered = float.Parse(parts[5]);
+
+                    servo.RecordTimePoint();
                 }
                 catch (Exception e)
                 {
@@ -192,7 +194,6 @@ public ViewModel Model
 
                 _model = value;
 
-                // ReSharper disable once InvertIf
                 if (Model != null)
                 {
                     Model.PropertyChanged += ModelOnPropertyChanged;
@@ -248,6 +249,7 @@ private void ServosOnCollectionChanged(object sender, NotifyCollectionChangedEve
                     servo.PropertyChanged += ServoOnPropertyChanged;
 
             if (e.Action == NotifyCollectionChangedAction.Reset)
+                // it's probably empty, but just in case...
                 foreach (var servo in Model.Servos)
                     servo.PropertyChanged -= ServoOnPropertyChanged;
         }

ServoPIDControl/Helper/README_NLog_WpfRichTextBox.txt

@@ -2,7 +2,7 @@
 using System.Windows;
 using System.Windows.Data;
 
-namespace ServoPIDControl
+namespace ServoPIDControl.Helper
 {
     public class VisibilityToCheckedConverter : IValueConverter
     {

…DControl/VisibilityToCheckedConverter.cs …l/Helper/VisibilityToCheckedConverter.csServoPIDControl/VisibilityToCheckedConverter.cs renamed to ServoPIDControl/Helper/VisibilityToCheckedConverter.cs ServoPIDControl/VisibilityToCheckedConverter.cs renamed to ServoPIDControl/Helper/VisibilityToCheckedConverter.cs

@@ -7,13 +7,14 @@
                       xmlns:d3="clr-namespace:InteractiveDataDisplay.WPF;assembly=mpolewaczyk.InteractiveDataDisplay.WPF"
                       xmlns:local="clr-namespace:ServoPIDControl"
                       xmlns:model="clr-namespace:ServoPIDControl.Model"
+                      xmlns:helper="clr-namespace:ServoPIDControl.Helper"
                       mc:Ignorable="d"
                       Title="Arduino Servo PID Control">
     <Window.DataContext>
         <model:ViewModel x:Name="Model" />
     </Window.DataContext>
     <Window.Resources>
-        <local:VisibilityToCheckedConverter x:Key="VisibilityToCheckedConverter" />
+        <helper:VisibilityToCheckedConverter x:Key="VisibilityToCheckedConverter" />
     </Window.Resources>
     <Grid>
         <Grid.ColumnDefinitions>

ServoPIDControl/MainWindow.xaml

@@ -92,7 +92,7 @@ private void ConnectNLogToUi()
                 AutoScroll = true,
                 // ReSharper disable StringLiteralTypo
                 Layout = "${processtime} [${level:uppercase=true}] " +
-                         "${logger:shortName=true}: ${message}" +
+                         "${logger:shortName=true}: ${message} " +
                          "${exception:innerFormat=tostring:maxInnerExceptionLevel=10:separator=,:format=tostring}",
                 // ReSharper restore StringLiteralTypo
             };

ServoPIDControl/MainWindow.xaml.cs

@@ -1,17 +1,18 @@
 using System;
-using System.Collections;
 using System.Collections.Generic;
 using System.Collections.ObjectModel;
 using System.ComponentModel;
+using System.Diagnostics;
 using System.Linq;
 using System.Runtime.CompilerServices;
-using InteractiveDataDisplay.WPF;
 using ServoPIDControl.Annotations;
 
 namespace ServoPIDControl.Model
 {
     public class ServoPidModel : INotifyPropertyChanged
     {
+        public static float TimeSeriesLengthSec = 5.0f;
+
         private float _p;
         private float _i;
         private float _d;
@@ -24,24 +25,33 @@ public class ServoPidModel : INotifyPropertyChanged
         private float _integrator;
         private float _dFiltered;
 
+        private readonly Stopwatch _stopWatch = Stopwatch.StartNew();
+        private readonly object _timeSeriesLock = new object();
+
         public ServoPidModel(int id)
         {
             Id = id;
 
 #if DEBUG
-            Times = new ObservableCollection<float>(Enumerable.Range(0, 500).Select(i => i / 100.0f));
-            SetPoints = new ObservableCollection<float>(Enumerable.Range(id * 100, 500)
-                .Select(i => i / 100 % 2 == 0 ? 80.0f : 100.0f));
-            Inputs = new ObservableCollection<float>(Enumerable.Range(id * 100, 500)
-                .Select(i => 90 + (float) Math.Sin(i / 50.0f)));
-            Outputs = new ObservableCollection<float>(Enumerable.Range(id * 100, 500)
-                .Select(i => 90 + (float) Math.Cos(i / 50.0f)));
+            lock (_timeSeriesLock)
+            {
+                const int ts = 500;
+
+                Times = Enumerable.Range(0, ts)
+                    .Select(i => i / 100.0f).ToList();
+                SetPoints = Enumerable.Range(id * 100, ts)
+                    .Select(i => i / 100 % 2 == 0 ? 80.0f : 100.0f).ToList();
+                Inputs = Enumerable.Range(id * 100, ts)
+                    .Select(i => 90 + (float) Math.Cos(i / 20.0f) * 9).ToList();
+                Outputs = Enumerable.Range(id * 100, ts)
+                    .Select(i => 90 + (float) Math.Sin(i / 20.0f) * 5).ToList();
+            }
 #endif
         }
 
         public int Id { get; }
 
-        public float  P
+        public float P
         {
             get => _p;
             set
@@ -163,26 +173,35 @@ internal set
         }
 
         // ReSharper disable MemberInitializerValueIgnored
-        public ObservableCollection<float> Times { get; } = new ObservableCollection<float>();
-        public ObservableCollection<float> SetPoints { get; } = new ObservableCollection<float>();
-        public ObservableCollection<float> Inputs { get; } = new ObservableCollection<float>();
-        public ObservableCollection<float> Outputs { get; } = new ObservableCollection<float>();
+        public List<float> Times { get; } = new List<float>();
+        public List<float> SetPoints { get; } = new List<float>();
+        public List<float> Inputs { get; } = new List<float>();
+
+        public List<float> Outputs { get; } = new List<float>();
         // ReSharper restore MemberInitializerValueIgnored
 
         public struct TimeSeries
         {
-            public ObservableCollection<float> X;
-            public ObservableCollection<float> Y;
+            public float[] X;
+            public float[] Y;
             public string Name;
         }
 
         public IEnumerable<TimeSeries> AllTimeSeries
         {
             get
             {
-                yield return new TimeSeries {X = Times, Y = SetPoints, Name = "SetPoint"};
-                yield return new TimeSeries {X = Times, Y = Inputs, Name = "Input"};
-                yield return new TimeSeries {X = Times, Y = Outputs, Name = "Output"};
+                lock (_timeSeriesLock)
+                {
+                    if (!Times.Any())
+                        yield break;
+
+                    var timeArray = Times.ToArray();
+
+                    yield return new TimeSeries {X = timeArray, Y = SetPoints.ToArray(), Name = "SetPoint"};
+                    yield return new TimeSeries {X = timeArray, Y = Inputs.ToArray(), Name = "Input"};
+                    yield return new TimeSeries {X = timeArray, Y = Outputs.ToArray(), Name = "Output"};
+                }
             }
         }
 
@@ -193,5 +212,24 @@ protected virtual void OnPropertyChanged([CallerMemberName] string propertyName
         {
             PropertyChanged?.Invoke(this, new PropertyChangedEventArgs(propertyName));
         }
+
+        public void RecordTimePoint()
+        {
+            lock (_timeSeriesLock)
+            {
+                Times.Add((float) _stopWatch.Elapsed.TotalSeconds);
+                SetPoints.Add(SetPoint);
+                Inputs.Add(Input);
+                Outputs.Add(Output);
+
+                var lastTime = Times.Last();
+                var removeCount = Times.TakeWhile(t => lastTime - t > TimeSeriesLengthSec).Count();
+
+                Times.RemoveRange(0, removeCount);
+                SetPoints.RemoveRange(0, removeCount);
+                Inputs.RemoveRange(0, removeCount);
+                Outputs.RemoveRange(0, removeCount);
+            }
+        }
     }
 }

ServoPIDControl/Model/ServoPidModel.cs

@@ -91,7 +91,7 @@
     <Compile Include="ISerialPort.cs" />
     <Compile Include="Properties\Annotations.cs" />
     <Compile Include="Model\ServoPidModel.cs" />
-    <Compile Include="VisibilityToCheckedConverter.cs" />
+    <Compile Include="Helper\VisibilityToCheckedConverter.cs" />
     <Page Include="MainWindow.xaml">
       <Generator>MSBuild:Compile</Generator>
       <SubType>Designer</SubType>
@@ -141,8 +141,5 @@
   <ItemGroup>
     <Folder Include="Tests\" />
   </ItemGroup>
-  <ItemGroup>
-    <Resource Include="Helper\README_NLog_WpfRichTextBox.txt" />
-  </ItemGroup>
   <Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
 </Project>