Приветствую! Посмотрел ваш код, неплохо, понравилось что вы прерывания любите это очень хорошо. Я бы конечно еще кучу проверок бы добавил, ну не суть. Тут недавно обсуждали для вездехода эбу и вот какие ключевые моменты для себя решили (может вам пригодится идейка)что надо делать не по топологии "звезда" а на шине общей для экономии проводов и повышению надежности, ранее использовалась CAN шина, теперь FlexRay (строгая по времени и ресурсам) шина. То есть делаем мелкую, универсальную, исполнительную, оконечную плату на десяток вводов выводов на базе дешевого контроллера stm32 и к ней подцепляем датчики или твердотельные выключатели (да что угодно). В качестве этого контроллера используем STM32 а не атмеги по причине того что STM32 стоит !дешевле! arduino NANO, является ARM7 с кучей памяти и прерываний, имеет встроенную CAN шину. И всеми этими оконечными устройствами управляем по шине c продублированного и экранированного головного устройства.
Похожая реализация например используется у наших друзей в камаз-мастер c CANNY7.
https://www.canny.ru/industries/sport/sportcarmod/
И вторая мысль: логически и физически разделяем вездеход/трактор на три независимых раздела чтобы вездеход был максимально надежный и мог выполнить свою миссию в ручном режиме или добраться до дома в случае отказа какого либо из устройств + можно было на ходу менять физически отломанные устройства:
1 - управление двигателем или силовой установкой/генератором. (то что начали вы)
2 - управление вспомогательными устройствами помогающими выжить технике (безопасность, выживание, отопление, все проверки агрегатов )
3 - навигацонный блок (gps, автопилот, лидары, камеры, карты и тп) (вот посмотрите видео что я имею ввиду под третим блоком)
https://youtu.be/tradNtWvPS4
