public ControlLoop(
ILoopWorkerFactory loopWorkerFactory,
ControlPoint pipe,
IMovementHost movementHost,
IHostApplicationLifetime lifetime,
ILogger <ControlLoop>?logger)
{
if (loopWorkerFactory is null)
{
throw new ArgumentNullException(nameof(loopWorkerFactory));
}
if (movementHost is null)
{
throw new ArgumentNullException(nameof(movementHost));
}
_pipe = pipe ?? throw new ArgumentNullException(nameof(pipe));
_lifetime = lifetime ?? throw new ArgumentNullException(nameof(lifetime));
_loopWorker = loopWorkerFactory.Create(nameof(ControlLoop), LoopDelegate);
_logger = logger;
void LoopDelegate(CancellationToken token)
{
try
{
_pipe().Wait();
movementHost.Invoke();
_logger?.LogTrace($"Loop run finished.");
}
catch (Exception e)
{
//Todo: add logging and catch logic.
_logger?.LogError(e, $"Loop run crashed add time: {DateTime.Now}");
}
}
}
//private IMotorController? _motorController;
//private IImuSensor? _imuSensor;
public Stabilizer(
ILoopWorkerFactory workerFactory,
IPidRegulatorFactory regulatorFactory,
ISettings setting,
IMovementTarget target,
IHostApplicationLifetime lifetime,
ILogger <Stabilizer>?logger)
{
if (workerFactory is null)
{
throw new ArgumentNullException(nameof(workerFactory));
}
if (regulatorFactory is null)
{
throw new ArgumentNullException(nameof(regulatorFactory));
}
if (setting is null)
{
throw new ArgumentNullException(nameof(setting));
}
_target = target ?? throw new ArgumentNullException(nameof(target));
_lifetime = lifetime ?? throw new ArgumentNullException(nameof(lifetime));
_logger = logger;
//hardwareManager.ConnectAsync().ContinueWith(result =>
//{
// if (result.Result)
// {
// _motorController = hardwareManager.GetHardware<IMotorController>()
// ?? throw new ArgumentException($"Can't resolve object of type{nameof(IMotorController)}");
// _imuSensor = hardwareManager.GetHardware<IImuSensor>()
// ?? throw new ArgumentException($"Can't resolve object of type{nameof(IImuSensor)}");
// }
//}, TaskScheduler.Default);
_worker = workerFactory.Create("StabilizerLoop", Callback);
var opt = setting.GetBinding <PidRegulatorSetting>("yAxisRegulator");
opt.DGain = 4;
_xAxisRegulator = regulatorFactory.Create("xAxisRegulator");
_yAxisRegulator = regulatorFactory.Create("yAxisRegulator");
}