Гравировка на RAMPS'

Доброго времени суток. ;) Собрал станок с кинематикой CoreXY на mega2560 с рампсом 1.4 сверху. Хочу этим станком гравировать. Проблема в том, что гравировка получается только на ровных поверхностях без всяких впадин. На станке стоит прошивка Marlin. Читал как гравировать с помощью программы Candle на любой неровной поверхности. В этой программе есть такая полезная функция как 'карта высот'. Но дело в том что с прошивкой Marlin станок в этой программе как бы подключается но не реагирует ни на какие команды с программы. Реагирует только на команды с Repetierhost'а. Скорость порта ставил (у меня 115200 везде). Пробовал шить GRBL 0.9 - то же самое, только при подаче питания на рампс постоянно горит индикатор питания экструдера на рампсе. Кстати нашел GRBL 0,9 именно для mega2560. Но не удобно потому что на этой прошивке дисплей 12864 не показывает ничего. На Repetier firmware та же песня - станок не реагирует на команды с Candle. Пробовал Universal Gcode Sender - опять же, плата подключается, но на любые команды шаговики не реагируют.

Подскажите пожалуйста какой то выход с ситуации, как можно заставить станок 'играть' столом по оси Z во время гравировки?