public static IEnumerable <double> MovingAverage(this IList <double> data, int windowSize)
{
if (windowSize.IsEven())
{
throw new ArgumentException("Window size must be odd");
}
if (data.Count == 0)
{
yield break;
}
var buffer = new CircularBuffer <double>(windowSize);
var halfWindowSize = (windowSize - 1) / 2;
var paddedData = Enumerable.Repeat(data.First(), halfWindowSize)
.Concat(data)
.Concat(Enumerable.Repeat(data.Last(), halfWindowSize))
.ToList();
paddedData.Take(windowSize - 1).ForEach(x => buffer.Put(x));
for (int dataIdx = 0; dataIdx < data.Count; dataIdx++)
{
buffer.Put(paddedData[windowSize - 1 + dataIdx]);
yield return(buffer.Average());
}
}
public float getRightSlider()
{
if (stream == null)
{
return(rightValue);
}
return(rightSliderBuffer.Average());
}
public float getLeftSlider()
{
if (stream == null)
{
return(leftValue);
}
return(leftSliderBuffer.Average());
}
public void ProcessFFT(float[] bands)
{
// Calculate which band the frequency is in
int idx = (int)(bands.Length * Frequency);
int bw = (int)(bands.Length * Bandwidth);
//TODO: bandwidth
int i1 = idx - bw / 2;
int i2 = idx + bw / 2;
if (i1 < 0)
{
i1 = 0;
}
if (i2 >= bands.Length)
{
i2 = bands.Length - 1;
}
i2++;
float value = 0.0f;
for (int i = i1; i < i2; i++)
{
value += bands[idx];
}
value /= (float)bw;
// Add sample to circular buffer
Buffer.Add(ValueThreshold(value, Threshold));
// Average
float average = CircularBuffer <float> .Average(Buffer); //TODO: Uncomment
PreIntensity = average;
// Weight & Limit
Intensity = Math.Min(average * Weight, Limit);
}
public HighPrecisionTimer(int intervalMs, bool startRunning = true)
{
this.log = Log.Logger;
this.log.Information("Starting HighPrecisionTimer with {0} ms interval", intervalMs);
if (intervalMs < 1)
{
throw new ArgumentOutOfRangeException();
}
System.Diagnostics.Trace.Assert(intervalMs >= 10, "Not reliable/tested, may use too much CPU");
this.IntervalMs = intervalMs;
this.cancelSource = new CancellationTokenSource();
this.taskComplete = new ManualResetEvent(false);
#if PROFILE
// Used to report timing accuracy for 1 sec, running total
tickTiming = new CircularBuffer.CircularBuffer <int>(1000 / intervalMs, true);
execTiming = new CircularBuffer.CircularBuffer <long>(1000 / intervalMs, true);
#endif
var watch = System.Diagnostics.Stopwatch.StartNew();
long durationMs = 0;
long totalTicks = 0;
long nextStop = intervalMs;
#if PROFILE
long lastReport = 0;
#endif
this.task = new Task(() =>
{
var eventArgs = new TickEventArgs();
while (!this.cancelSource.IsCancellationRequested)
{
long msLeft = nextStop - watch.ElapsedMilliseconds;
if (msLeft <= 0)
{
durationMs = watch.ElapsedMilliseconds;
totalTicks = durationMs / intervalMs;
#if PROFILE
var execWatch = System.Diagnostics.Stopwatch.StartNew();
#endif
var handler = Tick;
if (handler != null)
{
eventArgs.Duration = TimeSpan.FromMilliseconds(durationMs);
eventArgs.TotalTicks = totalTicks;
handler(this, eventArgs);
if (eventArgs.Cancel)
{
break;
}
}
#if PROFILE
execWatch.Stop();
execTiming.Put(execWatch.ElapsedTicks);
tickTiming.Put((int)(durationMs - nextStop));
if (durationMs - lastReport >= 1000)
{
// Report
log.Debug("HighPTimer avg: {0:F1} best: {1} worst: {2} MaxExec: {3:N1}ms",
tickTiming.Average(), tickTiming.Min(), tickTiming.Max(),
TimeSpan.FromTicks(execTiming.Max()).TotalMilliseconds);
lastReport = durationMs;
}
#endif
// Calculate when the next stop is. If we're too slow on the trigger then we'll skip ticks
nextStop = intervalMs * (watch.ElapsedMilliseconds / intervalMs + 1);
continue;
}
else if (msLeft < 16)
{
System.Threading.SpinWait.SpinUntil(() => watch.ElapsedMilliseconds >= nextStop);
continue;
}
System.Threading.Thread.Sleep(1);
}
this.taskComplete.Set();
}, cancelSource.Token, TaskCreationOptions.LongRunning);
if (startRunning)
{
this.task.Start();
}
}
public async Task ApproachNearestVisibleTrafficCone(DetectedObject nearestDetectedObject)
{
// https://imgur.com/a/KwNsgFa
// https://imgur.com/a/ugY9CRE 6m distance & bbox >= 33%
/*
* [17:51, 5/8/2020] Robin Derungs: 1. Auf 42% korrigieren
* Fahren bis bbox >=33% über 5-6 samples
* Dann erneut ausrichten auf 33% und solange fahren bis links pylon durch tof
* [17:51, 5/8/2020] Robin Derungs: 2. Sobald links erkannt, fahre noch ca. 50cm weiter.
* Falls tilt not horizontal, fahre weiter bis horizontal
* [17:53, 5/8/2020] Robin Derungs: 3. Drehe bis ein neuer pylon >= 50% pic height gefunden.
* [17:55, 5/8/2020] Robin Derungs: 4. Keiner gefunden? Dh wird durch den aktuellen verdeckt. Drehe 45 grad links zu. Fahre fort mit 5.
* Gefunden? Fahre fort mit 1.
* [17:56, 5/8/2020] Robin Derungs: 5. Fahre 1 m
* [17:56, 5/8/2020] Robin Derungs: 6. Fahre fort mit 3.
*/
// Ausrichten 42% +/- 2%
while (!this.GetAnglePercentOfTrafficConeCenter(nearestDetectedObject).IsWithin(0.40, 0.44))
{
await this.motorController.TurnLeftAsync();
nearestDetectedObject = this.objectDetectionController.GetNearestDetectedTrafficCone();
}
// bbox size can vary quite much frame by frame. so check the average size over 6 measurements
// use a circular buffer initialized with zeros so it requires all 6 measurements to be done
var circularBuffer = new CircularBuffer <double>(6, Enumerable.Repeat(0d, 6).ToArray());
while (circularBuffer.Average() < 0.33)
{
this.motorController.SetForward(MotorSpeed.Medium);
nearestDetectedObject = this.objectDetectionController.GetNearestDetectedTrafficCone();
circularBuffer.PushFront(this.GetBboxHeightPercentage(nearestDetectedObject));
}
// Ausrichten 33% +/- 2%
while (!this.GetAnglePercentOfTrafficConeCenter(nearestDetectedObject).IsWithin(0.31, 0.35))
{
await this.motorController.TurnLeftAsync();
nearestDetectedObject = this.objectDetectionController.GetNearestDetectedTrafficCone();
}
while (this.tofController.GetTofResult().DistanceInformation != DistanceInformation.TrafficConeDetected)
{
this.motorController.SetForward(MotorSpeed.Medium);
}
// drive approx 50cm. can be 40 or 60, doesnt really matter that much
this.motorController.SetForward(MotorSpeed.Slow);
await Task.Delay(TimeSpan.FromSeconds(2));
while (this.tiltController.GetTiltResult().OrientationInformation != OrientationInformation.Horizontal)
{
this.motorController.SetForward(MotorSpeed.Slow);
}
this.motorController.Stop();
}
public void Run()
{
using (var window = new NativeWindow(InitialGameWidth, InitialGameHeight, "Dragh 2.0"))
{
window.SizeChanged += SizeChanged;
window.MouseMoved += MouseMoved;
window.KeyAction += KeyAction;
using (var context = GenerateSkiaContext(window))
using (var skiaSurface = GenerateSkiaSurface(context, window.ClientSize))
{
var canvas = skiaSurface.Canvas;
var utility = _services.GetRequiredService <IUtilityService>();
var statsService = _services.GetRequiredService <IStatsService>();
var gameUpdateService = _services.GetRequiredService <IGameUpdateService>();
gameUpdateService.WindowSize = new Position(window.Size.Width / Block.BlockSize, window.Size.Height / Block.BlockSize);
var keyboard = _services.GetRequiredService <IKeyboardService>();
var world = _services.GetRequiredService <IWorldController>();
var hud = _services.GetRequiredService <HUDController>();
var entities = _services.GetRequiredService <EntityController>();
var screensController = _services.GetRequiredService <ScreensController>();
var frameDelay = 0;
var lastReadings = new List <int>();
var fpsReadings = new CircularBuffer <double>(10);
var targetFPS = 60;
var upperFPS = targetFPS + 5;
var lowerFPS = targetFPS - 5;
while (!window.IsClosing)
{
//canvas.ClipRect(new SKRect(0, 0, GameToWindowSize(gameUpdateService.WindowSize.X), GameToWindowSize(gameUpdateService.WindowSize.Y)), SKClipOperation.Intersect, true);
_framesTimer.Stop();
double milliseconds = _framesTimer.ElapsedMilliseconds;
_framesTimer.Restart();
var fps = 1 / (milliseconds / 1000);
fpsReadings.Add(fps);
var averageFps = fpsReadings.Average();
if (averageFps < lowerFPS)
{
frameDelay--;
}
if (averageFps > upperFPS)
{
frameDelay++;
}
statsService.SetFPS((int)averageFps);
if (frameDelay > 0)
{
Thread.Sleep(frameDelay);
}
Update(window, statsService, gameUpdateService, keyboard, world, hud, entities, screensController);
Render(window, canvas, utility, world, hud, entities, gameUpdateService.WindowSize, screensController);
Glfw.PollEvents();
if (keyboard.IsPressed(Key.ESCAPE))
{
break;
}
}
}
}
}
