Робот, що керується через Інтернет на .NET

В цій статті розкажу більше про структуру застосунку на .Net. Я не буду зупинятися на підключенні моторів, серво, налаштування PWM та інше. Я просто покажу своє бачення подібного застосунку, яке вдалося реалізувати в коді. Та, якщо до цього буде інтерес, процес опишу в іншій статті (більше як туторіал). Також в іншій статті опишу, як зробити video streaming за допомогою SignalR Streams.

Повний код з вище описаним вже можна побачити на GitHub.

Трішки про ідею та кота

В мене є кіт — Морті, який дуже сумує ̶т̶а̶ ̶б̶у̶х̶а̶є̶ коли лишається сам удома.

Щоб, власне, сонечко не сумувало, зʼявилася ідея робота, який би міг їздити по квартирі та розважати котика.

Як це виглядає?

Технології

Щоб вибрати правильні технології, треба спершу окреслити вимоги до робота:

1. Робот має їздити вперед-назад та повертати

2. Робот має транслювати відео з камери

3. Робот має повністю керуватися через Інтернет

За основу вибрано платформу AlphaBot Acce Pack. Вона вже поставляється з усім необхідним: моторами, серво для керування поворотом камери та, власне, камерою. Серцем робота стала плата Raspberry Pi 3.

Єдине що лишалося — написати програмне забезпечення. Я .Net розробник, тому для розробки я вибрав .Net, хоч це і не є типовим вибором для Raspberry Pi. Більшість DIY проєктів під неї пишеться на Python. Та я вирішив використати .Net не тільки тому, що я його знаю краще за Python, а ще й тому, що мені було цікаво, чи вдасться це повністю написати на C#. Спойлер — так, там є все необхідне.

Програмне забезпечення

Основна ідея: написати застосунок, який складається з незалежних частин, що взаємодіють між собою за допомогою команд.

Command Bus

На початку я взявся за написання шини команд (Command Bus).

Серцем шини є Channels. Це імплементація паттерну producer/consumer, що працює асинхронічно та ідеально підходить до цієї задачі. Кожен Channel має Reader і Writer, для читання і запису відповідно.

Для публікації команд я створив ICommandPublisher. Для того щоб підписатися на команду — ICommandSubscriber. Виглядає ось так:

Для ”зберігання” підписок є окремий репозиторій, а викликом підписок займається клас CommandBus:

    public class CommandBus : ICommandBus
    {
        private readonly ChannelReader<(string Command, object? Message)> _channelReader;
        private readonly ICommandSubscribersRepository _subscribersRepository;

        public CommandBus(
            ChannelReader<(string Command, object? Message)> channelReader,
            ICommandSubscribersRepository subscribersRepository)
        {
            _channelReader = channelReader;
            _subscribersRepository = subscribersRepository;
        }

        public async Task ProcessCommandsAsync(CancellationToken cancellationToken)
        {
            try
            {
                await ProcessCommandsStreamAsync(cancellationToken);
            }
            catch(TaskCanceledException)
            {
                ColoredConsole.WriteLineRed("Commands processing was stopped.");
            }
        }

        private async Task ProcessCommandsStreamAsync(CancellationToken cancellationToken)
        {
            await foreach (var command in _channelReader.ReadAllAsync(cancellationToken))
            {
                InvokeCommandSubscribersActions(command);
            }
        }

        private void InvokeCommandSubscribersActions((string Command, object? Message) command)
        {
            var actions = _subscribersRepository.Get(command.Command);
            actions.ForEach(action => action(command.Message));
        }
    }

Command Bus запускається в окремому потоці і працює до моменту зупинки застосунку.

Controllers

Як я писав вище, головною ідеєю було розбити застосунок на незалежні частини, що взаємодіють між собою. Це — контролери. Кожен може підписуватись та відправляти команди. Також, якщо контролер має виконувати роботу постійно (наприклад очікує команди з консолі), то він запускається в окремому потоці. Це було свідоме рішення, щоб в майбутньому можна було б розвивати застосунок (наприклад, додати автодетекцію перешкод і відправляти сигнал стоп, якщо попереду вона є). Застосунок зупиниться, якщо всі контролери зупиняться, тому повинен бути хоча б один, що працює постійно. Ним може бути ConsoleController (що очікує команди з консолі)

[ControllerConfiguration("Console")]
    public class ConsoleController : ControllerBase, IController
    {
        private readonly ICommandPublisher _commandPublisher;

        private string _lastMoveCommand = string.Empty;
        private bool _exitDetected = false;

        public ConsoleController(ICommandPublisher commandPublisher)
        {
            _commandPublisher = commandPublisher;
        }

        public async override Task HandleAsync(CancellationToken cancellationToken)
        {
            ColoredConsole.WriteLineGreen("Console controller started. Press 'Q' to exit.");

            while (!cancellationToken.IsCancellationRequested && !_exitDetected)
            {
                await ProcessConsoleInputAsync();
            }
        }

        private async Task ProcessConsoleInputAsync()
        {
            Console.Write("\nPlease, provide command key: ");
            var key = Console.ReadKey();

            switch (key.Key)
            {
                case ConsoleKey.A:
                    await SendMoveCommand(MoveControllerCommands.Left);
                    break;

                case ConsoleKey.D:
                    await SendMoveCommand(MoveControllerCommands.Right);
                    break;

                case ConsoleKey.S:
                    await SendMoveCommand(MoveControllerCommands.Stop, MoveControllerCommands.Back);
                    break;

                case ConsoleKey.W:
                    await SendMoveCommand(MoveControllerCommands.Ahead, MoveControllerCommands.Stop);
                    break;

                case ConsoleKey.LeftArrow:
                    await SendCameraCommand(CameraNeckControllerCommands.CameraLeft);
                    break;

                case ConsoleKey.RightArrow:
                    await SendCameraCommand(CameraNeckControllerCommands.CameraRight);
                    break;

                case ConsoleKey.UpArrow:
                    await SendCameraCommand(CameraNeckControllerCommands.CameraAhead);
                    break;

                case ConsoleKey.Q:
                    await Exit();
                    break;

                default:
                    break;
            }
        }

        private async Task SendMoveCommand(string newCommand, string rebaseCommand = MoveControllerCommands.Stop)
        {
            if (_lastMoveCommand == newCommand)
            {
                newCommand = rebaseCommand;
            }

            ColoredConsole.WriteLineCyan($"\nSent command: {newCommand}");
            await _commandPublisher.PublishAsync(newCommand);
            _lastMoveCommand = newCommand;
        }

        private async Task SendCameraCommand(string command)
        {
            ColoredConsole.WriteLineCyan($"\nSent command: {command}");
            await _commandPublisher.PublishAsync(command);
        }

        private async Task Exit()
        {
            ColoredConsole.WriteLineCyan("\nSent command: Exit");
            await _commandPublisher.PublishAsync("Exit");
            _exitDetected = true;
        }
    }
}

або ж LoopController (безкінечний цикл, що потрібен для запуску застосунку як Service в Linux, так як ввід з консолі в такому випадку не підтримується).

	[ControllerConfiguration("Loop")]
	public class LoopController : ControllerBase, IController
    {
		public LoopController()
		{
		}

        public async override Task HandleAsync(CancellationToken cancellationToken)
        {
            await Task.Yield();
            while(!cancellationToken.IsCancellationRequested)
            {
                // Loop
            }
        }
    }

ControllerConfiguration - атрибут, який вказує секцію конфігурації контролера. В конфігурації можна вказати який контролер запускати, а який ні.

Контролери CameraNeckController та MoveController відповідають за керування поворотом камери та моторами відповідно.

public class CameraNeckController : ControllerBase
    {
        private readonly ICameraNeck _cameraNeck;

        public CameraNeckController(IHardwareProvider provider, ICommandSubscriber commandSubscriber)
        {
            _cameraNeck = provider.GetRequiredHardware<ICameraNeck>();
            commandSubscriber.Subscribe(CameraNeckControllerCommands.CameraLeft, OnCameraLeft);
            commandSubscriber.Subscribe(CameraNeckControllerCommands.CameraRight, OnCameraRight);
            commandSubscriber.Subscribe(CameraNeckControllerCommands.CameraAhead, OnCameraAhead);
            commandSubscriber.Subscribe(CameraNeckControllerCommands.CameraAngle, OnCameraAngle);
        }

        private void OnCameraAngle(object? message)
        {
            var cameraAngleMessage = (CameraAngleMessage)message!;
            _cameraNeck.WriteXAngle(cameraAngleMessage.CameraAngleX);
            _cameraNeck.WriteYAngle(cameraAngleMessage.CameraAngleY);
        }

        private void OnCameraAhead(object? _)
        {
            _cameraNeck.TurnAhead();
        }

        private void OnCameraRight(object? _)
        {
            _cameraNeck.TurnRightMax();
        }

        private void OnCameraLeft(object? _)
        {
            _cameraNeck.TurnLeftMax();
        }
    }

public sealed class MoveController : ControllerBase
    {
        private readonly IDriver _driver;

        public MoveController(IHardwareProvider provider, ICommandSubscriber commandSubscriber)
        {
            _driver = provider.GetRequiredHardware<IDriver>();

            commandSubscriber.Subscribe(MoveControllerCommands.Ahead, OnAhead);
            commandSubscriber.Subscribe(MoveControllerCommands.Stop, OnStop);
            commandSubscriber.Subscribe(MoveControllerCommands.Left, OnLeft);
            commandSubscriber.Subscribe(MoveControllerCommands.Right, OnRight);
            commandSubscriber.Subscribe(MoveControllerCommands.Back, OnBack);
            commandSubscriber.Subscribe(MoveControllerCommands.SpeedWithDirection, OnSpeedWithDirection);
        }

        private void OnSpeedWithDirection(object? message)
        {
            var speedWithDirectionMessage = (SpeedWithDirectionMessage)message!;
            _driver.SetSpeedWithDirection(speedWithDirectionMessage.Speed, speedWithDirectionMessage.Direction);
        }

        private void OnBack(object? _)
        {
            _driver.GoBack();
        }

        private void OnRight(object? _)
        {
            _driver.GoRight();
        }

        private void OnLeft(object? _)
        {
            _driver.GoLeft();
        }

        private void OnStop(object? _)
        {
            _driver.Stop();
        }

        private void OnAhead(object? _)
        {
            _driver.GoAhead();
        }
    }

Hardware

Hardware - це наступний шар застосунку робота. На цьому рівні ми можемо створювати класи які безпосередньо керують залізом нашого робота. Кожне залізо скоріш за все вимагає ініціалізації (конфігурація пінів), тож кожен клас, який буде працювати з залізом, має наслідуватись від HardwareBase та може реалізовати методи Initialize та OnStop. Приклад:

    public class Driver : HardwareBase, IDriver
    {
        private readonly DriverSettings _settings;

        private DCMotor? _leftMotor;
        private DCMotor? _rightMotor;

        public Driver(DriverSettings settings) => _settings = settings;

        public override void Initialize(GpioController controller)
        {
            _leftMotor = DCMotor.Create(_settings.ENA, _settings.IN2, _settings.IN1, controller);
            _rightMotor = DCMotor.Create(_settings.ENB, _settings.IN4, _settings.IN3, controller);

            Stop();
        }

        public override void OnStop(GpioController gpioController)
            => Stop();


        public void GoAhead()
        {
            SetSpeedWithDirection(0.5, 0);
        }

        public void GoLeft()
        {
            SetSpeedWithDirection(0.5, -1);
        }

        public void GoRight()
        {
            SetSpeedWithDirection(0.5, 1);
        }

        public void Stop()
        {
            SetSpeedWithDirection(0, 0);
        }

        public void GoBack()
        {
            SetSpeedWithDirection(-0.5, 0);
        }

        public void SetSpeedWithDirection(double speed, double direction)
        {
            var motorSpeeds = DriverSpeedConverter
                .Convert(speed, direction, _settings.CalibrationCoefficient);

            _leftMotor!.Speed = motorSpeeds.LeftMotorSpeed;
            _rightMotor!.Speed = motorSpeeds.RightMotorSpeed;
        }
    }

Кожен компонент Hardware реєструється в системі, і під час запуску застосунку, для кожного компоненту викликається метод Initialize. Цим займається HardwareBootstrapper.

Дешеві серво

Дешеві серво - велика проблема, вони ніколи не працюють ідеально, одне серво може працювати добре, а інше сіпатися як тільки ти змінюєш кут. Для того щоб серво не сіпалося, було прийняте рішення його вимикати після того як користувач змінить кут, для цього я створив огортку над звичайним серво AutoDisabledServo

public class AutoDisabledServo : IDisposable
	{
        private bool _disposed = false;

        private readonly int _delayInMillisecondsBeforeStopServo;

        private bool _servoIsStarted;

        private readonly ServoMotor _servo;
        private readonly Timer _disableServoTimer;

        private int _previousAngle = int.MinValue;
        private readonly bool _reverseAngle;

        public AutoDisabledServo(ServoMotor servo, int delayInMilliseconds = 500, bool reverseAngle = false)
		{
			_servo = servo;
            _delayInMillisecondsBeforeStopServo = delayInMilliseconds;
            _disableServoTimer = new Timer(DisableServo, null, Timeout.Infinite, Timeout.Infinite);
            _reverseAngle = reverseAngle;
        }

        public void WriteAngle(int angle)
        {
            if (_previousAngle != angle)
            {
                _previousAngle = angle;

                if (_reverseAngle)
                    angle = 180 - angle;

                StartServo();
                _servo?.WriteAngle(angle);
                _disableServoTimer?.Change(_delayInMillisecondsBeforeStopServo, Timeout.Infinite);
            }
        }

        private void DisableServo(object? state)
        {
            if (_servoIsStarted)
            {
                _disableServoTimer?.Change(Timeout.Infinite, Timeout.Infinite);
                _servo?.Stop();
                _servoIsStarted = false;
            }
        }

        private void StartServo()
        {
            if (!_servoIsStarted)
            {
                _servo?.Start();
                _servoIsStarted = true;
            }
        }

        public void Dispose()
        {
            Dispose(true);
            GC.SuppressFinalize(this);
        }

        protected virtual void Dispose(bool disposing)
        {
            if (_disposed) return;

            _servo.Stop();
            _servo.Dispose();

            _disposed = true;
        }

        ~AutoDisabledServo()
        {
            Dispose(false);
        }
    }

Мотори

Робот має їздити вперед/назад та вміти плавно повертати. Можливість повороту досягається тим, що один мотор працює менш потужно ніж інший. Для зміни напряму робот дістає два значення — швидкість та напрям. Кожне значення - це діапазон від -1 до 1. Для швидксті - від’ємні значення це рух назад, додатні - вперед. Для напрямку - рух вліво/вправо відповідно.

Але отриманні значення треба трансформувати в швідкість окремо правого та лівого мотору, для цього є клас DriverSpeedConverter:

	public static class DriverSpeedConverter
	{
        /// <summary>
        /// Converts a speed and direction into left and right motor speeds.
        /// </summary>
        /// <param name="speed">Speed value ranging from -1 to 1:
        /// -1 to 0 for moving backward, 0 to 1 for moving forward.</param>
        /// <param name="direction">Direction value ranging from -1 to 1:
        /// -1 to 0 for turning left, 0 to 1 for turning right.</param>
        /// <param name="calibrationCoefficient">A coefficient indicating the calibration factor of the right motor compared to the left motor.
        /// Values greater than 1 make the right motor weaker, and values less than 1 make the right motor stronger.</param>
        /// <returns>A tuple containing the left and right motor speeds.</returns>
        public static (double LeftMotorSpeed, double RightMotorSpeed) Convert(double speed, double direction, double calibrationCoefficient = 1)
		{
            var rightMotorCoefficient = direction > 0 ? 1 - direction : 1;
            var leftMotorCoefficient = direction < 0 ? 1 + direction : 1;

            leftMotorCoefficient = leftMotorCoefficient * calibrationCoefficient;

            return (leftMotorCoefficient * speed, rightMotorCoefficient * speed);
        }
	}

calibrationCoefficient - параметр, що визначає наскільки лівий мотор сильніший чи cлабший за правий.

Комунікація з завнішнім світом

Як я описував вище, роботом можна керувати з консолі. Але основна ідея була в тому, щоб керувати роботом через Інтернет, наприклад, через мобільний застосунок React Native (який в мене є, але я вам його не віддам 😁). Для цього ідеально підходять Web Sockets та фреймворк, який з ними працює - SignalR.

Для отримання команд з SignalR є контроллер SignalRCommandController.

public class SignalRCommandController : ControllerBase
    {
        private readonly HubConnection _hubConnection;
        private readonly ICommandPublisher _commandPublisher;

        public SignalRCommandController(ICommandPublisher commandPublisher, HubConnection hubConnection)
        {
            _hubConnection = hubConnection;
            _commandPublisher = commandPublisher;

            _hubConnection.On<double, double>("ReceiveSpeedAndDirection", ReceiveSpeedAndDirection);
            _hubConnection.On<int, int>("ReceiveCameraAngle", ReceiveCameraAngle);
        }

        private void ReceiveSpeedAndDirection(double speed, double direction)
        {
            ColoredConsole.WriteLineCyan($"Received speed: {speed}, direction: {direction} from SignalR.");
            _commandPublisher.Publish(MoveControllerCommands.SpeedWithDirection, SpeedWithDirectionMessage.Of(speed, direction));
        }

        private void ReceiveCameraAngle(int cameraAngleX, int cameraAngleY)
        {
            ColoredConsole.WriteLineCyan($"Received camera angle X: {cameraAngleX}, Y: {cameraAngleY} from SignalR.");
            _commandPublisher.Publish(CameraNeckControllerCommands.CameraAngle, new CameraAngleMessage(cameraAngleX, cameraAngleY));
        }
    }

Все дуже просто: отримуємо команду від сервера і публікуємо її через ICommandPublisher.

Життєвий цикл застосунку

Життєвий цикл наступний:

1. Ініціалізація застосунку

2. Ініціалізація заліза

3. Запуск шини команд

4. Запуск SignalR

5. Запуск контролерів

6. Зупинка заліза, після того, як контролери виконали свою роботу

7. Зупинка застосунку

Що в підсумку

Код може видатися дещо складним, тим паче для такої простої ідеї. Та я писав його з думкою про те, що буду проєкт розвивати: додавати автодетекцію перешкод та інші плюшки.

Чи задоволений я результатом? Так, але є ще багато, що можна покращити. Дешеві серво працюють погано, а один мотор потужніший за інший (що можна пофіксити програмно). Та у комплексі це все працює та працює добре.

Чи можна цим роботом гратися з котом? Так, але до моменту, поки ваш кіт не перегризе шлейф до камери і ви не втратите відеозв’язок 😁