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();
}
public void DisableTorqueOnMotors()
{
Driver.DisableMotors();
}