Skip to content

Instantly share code, notes, and snippets.

@evgeniyp
Created December 11, 2015 12:13
Show Gist options
  • Save evgeniyp/5d7e92ec8df5de78e1d2 to your computer and use it in GitHub Desktop.
Save evgeniyp/5d7e92ec8df5de78e1d2 to your computer and use it in GitHub Desktop.
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;
}
}
}
<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>
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());
}
}
}
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