private async Task ExecuteMove(LegPositions targetPosition)
{
while (!_lastWrittenPosition.MoveFinished(targetPosition))
{
_lastWrittenPosition = _lastWrittenPosition.MoveTowards(targetPosition, Speed * 0.001f * _updateDelay);
_driver.MoveLegsSynced(_lastWrittenPosition);
await Task.Delay(_updateDelay);
}
}
protected override void EngineSpin()
{
// if last telemetrics was too long time ago fire
var currentTickCount = Environment.TickCount;
if (currentTickCount - _lastTelemetricsUpdate > TelemetricsUpdateInterval)
{
_lastTelemetricsUpdate = currentTickCount;
NewTelemetricsUpdate?.Invoke(this, Driver.ReadTelemetrics());
}
if (_lastWrittenPosition.MoveFinished(_nextMove ?? _lastWrittenPosition))
{
if (_moves.TryDequeue(out var deqeueuedLegPosition))
{
_moveQueueSingal.Reset();
_nextMove = deqeueuedLegPosition;
}
else
{
if (_moves.IsEmpty)
{
_moveQueueSingal.Set();
}
return;
}
}
try
{
_lastWrittenPosition = _lastWrittenPosition.MoveTowards(_nextMove, NextStepLength);
Driver.MoveLegsSynced(_lastWrittenPosition);
}
catch (IOException e)
{
GaitEngineError?.Invoke(this, e);
}
}
