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); } }