private async Task MainControllerLoop()
{
await ExecuteMove(RelaxedStance);
var watch = new Stopwatch();
watch.Start();
while (_keepRunning)
{
if (Direction.IsZero() && Rotation == 0f)
{
if (_moving)
{
await StepToRelaxed(GetNextLegCombo());
_moving = false;
}
else
{
if (!_lastWrittenPosition.MoveFinished(RelaxedStance))
{
await ShiftToRelaxed();
}
else
{
await Task.Delay(_updateDelay);
}
}
}
else
{
await ExecuteStep(Direction, Rotation, GetNextLegCombo());
_moving = true;
}
if (watch.ElapsedMilliseconds > TelemetricsUpdateInterval)
{
watch.Restart();
NewTelemetricsUpdate?.Invoke(this, _driver.ReadTelemetrics());
}
}
_driver.DisableMotors();
}
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);
}
}