Прошу помощи разобраться в G-Code

Всем добрый вечер.

Прошивка Marlin 2.

Столкнулся с парой проблем при написании Gcode для моего проектика.

Задача следующая.

Имеются оси XYZ и 2 сервомашинки.

Мне необходимо после установки осей в определенное положение повернуть серву в определенное положение и оставить ее так до следующей команды.

Написал GCode для теста.

G28 x0 y0

M280 P0 S0

M280 P1 S0

G0 X227 F25000

M280 P0 S90

G0 Y15 F2000

G0 X0 F25000

G0 X50 F25000

M280 P0 S0

G0 Y0 F2000

G0 X227 F25000

M280 P0 S90

G0 Y15 F2000

G0 X0 Y30 F25000

G0 X50 F25000

G0 Y0 F2000

M280 P0 S0

G0 X227 F25000

M280 P0 S90

M280 P1 S90

M280 P1 S0

M280 P0 S0

M117 SERGE!!! TEST FINISHED!!!

G28 x0 y0

По коду после установки осей в положение "А" я поворачиваю серву на 90 градусов. После установки осей в положение "Б" поворачиваю серву в исходное положение.

В реальности после команды M280 P0 S90 марлин отрабатывает все команды M280 сразу и вне очереди G0 команд. То-есть серва поворачивается на 90градусов и сразу возвращается назад. Также это происходит одновременно с выполнением G0 команд. Тоесть движение по оси скажем Y еще не завершилось а серва уже начинает работать. Как я понял для G и M команд в марлин имеются 2 потока которые работают независимо друг от друга. Как мне вписать выполнение M команд в очередность выполнения G команд ?