Created
December 11, 2015 12:13
-
-
Save evgeniyp/5d7e92ec8df5de78e1d2 to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
namespace WpfSonarAnalyzer | |
{ | |
/// <summary> | |
/// https://en.wikipedia.org/wiki/Low-pass_filter | |
/// </summary> | |
public class Lowpass | |
{ | |
private double x, xPrev, | |
y, yPrev; | |
public Lowpass() | |
{ | |
x = xPrev = y = yPrev = double.NaN; | |
} | |
public double Next(double value, double dt, double cuttoff) | |
{ | |
var alpha = dt / (1 / (2 * System.Math.PI * cuttoff) + dt); | |
if (double.IsNaN(x)) | |
{ | |
x = xPrev = yPrev = value; | |
} | |
else | |
{ | |
xPrev = x; | |
yPrev = y; | |
} | |
x = value; | |
y = yPrev + alpha * (x - yPrev); | |
return y; | |
} | |
} | |
} |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
<Controls:MetroWindow x:Class="WpfSonarAnalyzer.MainWindow" | |
xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation" | |
xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml" | |
xmlns:d="http://schemas.microsoft.com/expression/blend/2008" | |
xmlns:mc="http://schemas.openxmlformats.org/markup-compatibility/2006" | |
xmlns:local="clr-namespace:WpfSonarAnalyzer" | |
mc:Ignorable="d" | |
xmlns:Controls="clr-namespace:MahApps.Metro.Controls;assembly=MahApps.Metro" | |
Title="MainWindow" Height="147" Width="525" BorderBrush="Black"> | |
<Grid> | |
<ProgressBar x:Name="Progress_1" Maximum="200" HorizontalAlignment="Left" Height="10" Margin="10,10,0,0" VerticalAlignment="Top" Width="497"/> | |
<ProgressBar x:Name="Progress_2" Maximum="200" HorizontalAlignment="Left" Height="10" Margin="10,25,0,0" VerticalAlignment="Top" Width="497"/> | |
<ProgressBar x:Name="Progress_3" Maximum="200" HorizontalAlignment="Left" Height="10" Margin="10,40,0,0" VerticalAlignment="Top" Width="497"/> | |
<Label x:Name="Label_Visitors" Content="Visitors: " Margin="10,75,115,0" VerticalAlignment="Top"/> | |
</Grid> | |
</Controls:MetroWindow> |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
using System; | |
using System.Windows.Threading; | |
using MahApps.Metro.Controls; | |
namespace WpfSonarAnalyzer | |
{ | |
public partial class MainWindow : MetroWindow | |
{ | |
private const double TIMER_INTERVAL = 50; | |
private DispatcherTimer _deviceTimer; | |
private Robots.Models.Riddler_v10.SerialPortRobot _robot; | |
private SonarAnalyzer _sonarAnalyzer; | |
public MainWindow() | |
{ | |
InitializeComponent(); | |
Robots.Communication.SerialPortConfiguration config = new Robots.Communication.SerialPortConfiguration(""); | |
config.Port = "COM8"; | |
config.Rate = new Robots.Communication.BaudRate("115200"); | |
_robot = new Robots.Models.Riddler_v10.SerialPortRobot(config); | |
Robots.Models.Riddler_v10.Parts.Senses senses = ((Robots.Models.Riddler_v10.Parts.Senses)_robot.Senses); | |
senses.OnSonarsUpdate += OnSonarsUpdate; | |
_sonarAnalyzer = new SonarAnalyzer(100, 5); | |
_deviceTimer = new DispatcherTimer(); | |
_deviceTimer.Interval = TimeSpan.FromMilliseconds(TIMER_INTERVAL); | |
_deviceTimer.Tick += _deviceTimer_Tick; | |
_deviceTimer.Start(); | |
} | |
private void _deviceTimer_Tick(object sender, EventArgs e) | |
{ | |
if (!_robot.Connected()) { _robot.Connect(); } | |
Title = _robot.Connected().ToString(); | |
_robot.Senses.Update(); | |
} | |
private void OnSonarsUpdate(Robots.Models.Riddler_v10.Parts.Senses senses) | |
{ | |
var left = senses.Sonar_Left.Value; | |
double newLeftValue = _sonarAnalyzer.SetValue(SonarPosition.Left, 1 / TIMER_INTERVAL, left); | |
Progress_1.Value = newLeftValue; | |
var front = senses.Sonar_Front.Value; | |
double newFrontValue = _sonarAnalyzer.SetValue(SonarPosition.Front, 1 / TIMER_INTERVAL, front); | |
Progress_2.Value = newFrontValue; | |
var right = senses.Sonar_Right.Value; | |
double newRightValue = _sonarAnalyzer.SetValue(SonarPosition.Right, 1 / TIMER_INTERVAL, right); | |
Progress_3.Value = newRightValue; | |
_sonarAnalyzer.Update(); | |
Label_Visitors.Content = string.Format("Visited: {0}, count: {1}", _sonarAnalyzer.IsVisitedNow(), _sonarAnalyzer.VisitorCount()); | |
} | |
} | |
} |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
namespace WpfSonarAnalyzer | |
{ | |
public enum SonarPosition | |
{ | |
Left, Front, Right | |
} | |
public class SonarAnalyzer | |
{ | |
private class SonarValue | |
{ | |
private Lowpass _lowpass = new Lowpass(); | |
public double _lastValue; | |
public double Get() | |
{ | |
return _lastValue; | |
} | |
public double Set(double value, double dt, double cutoff) | |
{ | |
_lastValue = _lowpass.Next(value, dt, cutoff); | |
return _lastValue; | |
} | |
public void Reset() | |
{ | |
_lastValue = 0; | |
} | |
} | |
private bool _isVisited, _lastIsVisited; | |
private int _visitorCount; | |
private double _threshold, | |
_cutoff; | |
private SonarValue _left = new SonarValue(), | |
_front = new SonarValue(), | |
_right = new SonarValue(); | |
public SonarAnalyzer(double threshold, double cutoff) | |
{ | |
_threshold = threshold; | |
_cutoff = cutoff; | |
_visitorCount = 0; | |
} | |
public double SetValue(SonarPosition position, double dt, double value) | |
{ | |
double result; | |
switch (position) | |
{ | |
case SonarPosition.Left: result = _left.Set(value, dt, _cutoff); break; | |
case SonarPosition.Front: result = _front.Set(value, dt, _cutoff); break; | |
case SonarPosition.Right: result = _right.Set(value, dt, _cutoff); break; | |
default: result = double.NaN; break; | |
} | |
return result; | |
} | |
public void Update() | |
{ | |
_isVisited = _left._lastValue < _threshold | |
|| _right._lastValue < _threshold | |
|| _front._lastValue < _threshold; | |
if (_isVisited && !_lastIsVisited) { _visitorCount++; } | |
_lastIsVisited = _isVisited; | |
} | |
public bool IsVisitedNow() | |
{ | |
return _isVisited; | |
} | |
public int VisitorCount() | |
{ | |
return _visitorCount; | |
} | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment