Как научиться изящно и грамотно программировать МК?
Сначала моделировать структуру кода, продумать взаимодействие обособленных частей - "карандашом на бумажке" в виде блок-схем, независимых "виртуальных машин", и только после того, как будет понятна вся структура будущего устройства, начинать писать код. А дальше только практика; и есть код, написанный несколько месяцев назад, кажется ужасным, значит, вы на верном пути)
Главное - не торопиться сразу кодить.
Посмотрите в сторону операционных систем реального времени, например freertos. При большом количестве задач, возлагаемых на микроконтроллер, без них сложно сделать что-то хорошо работающее и хорошо написанное.