Exemplo n.º 1
0
        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}");
                }
            }
        }
Exemplo n.º 2
0
        //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");
        }