How to implement servo in place of Z motor

caelf

I'm working on a pen plotter that runs on repetier firmware. The idea is that gcode would be generated with inkscape. The plotter uses the standard x/y axis and a servo for the Z axis. The servo lifts the pen off the paper when moving to a new starting point. My question is, how do I implement this into the firmware?

Repetier

Don't think of it as z. Think more like on/off.

For on add
M400 ; Wait moves to finish
M340 .... ; servo position
G4 P50 ; // wait for servo to position

And for off
M400 ; Wait moves to finish
M340 .... ; servo position off
G4 P50 ; // wait for servo to position

So just add these extra commands in the inkskape output at start/end of a stroke. Since moves are buffered it is important to sync moves with M400 and M340 needs a wait to give servo enough time to position pen.