public MockRobot()
{
_fakeTelemetricsTimer = new Timer
{
AutoReset = true,
Enabled = true,
Interval = 2000
};
_fakeTelemetricsTimer.Elapsed += (sender, args) =>
{
var servos = new List <ServoTelemetrics>();
for (int i = 0; i < _rng.Next(1, 12); i++)
{
servos.Add(new ServoTelemetrics
{
Id = i,
Load = _rng.Next(0, 200),
Temperature = _rng.Next(20, 50),
Voltage = (float)_rng.NextDouble() * 5 + 9
});
}
NewTelemetricsUpdate?.Invoke(this, new QuadrupedTelemetrics {
ServoTelemetrics = servos
});
};
}
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);
}
}