diff --git a/cnc/config.py b/cnc/config.py
index 4231f7c..261965f 100644
--- a/cnc/config.py
+++ b/cnc/config.py
@@ -76,6 +76,7 @@ STEPPER_STEP_PIN_E = 0
 if ENABLE_L293D:
     STEPPER_STEP_PINS_X = [22,27,17,4]
     STEPPER_STEP_PINS_Y = [12,16,20,21] 
+    PEN_PIN = 25
 
 STEPPER_DIR_PIN_X = 0
 STEPPER_DIR_PIN_Y = 0 
diff --git a/cnc/gmachine.py b/cnc/gmachine.py
index 776db32..033077b 100644
--- a/cnc/gmachine.py
+++ b/cnc/gmachine.py
@@ -33,6 +33,7 @@ class GMachine(object):
         self._absoluteCoordinates = 0
         self._plane = None
         self._fan_state = False
+        self._pen_state = False
         self._heaters = dict()
         self.reset()
         hal.init()
@@ -69,6 +70,10 @@ class GMachine(object):
         hal.fan_control(state)
         self._fan_state = state
 
+    def _pen(self, state):
+        hal.pen_control(state)
+        self._pen_state = state
+
     def _heat(self, heater, temperature, wait):
         # check if sensor is ok
         if heater == HEATER_EXTRUDER:
@@ -112,6 +117,7 @@ class GMachine(object):
             raise GMachineException("out of maximum speed")
 
     def _move_linear(self, delta, velocity):
+        logging.info("[BEFORE] Moving linearly {}".format(delta))
         delta = delta.round(1.0 / STEPPER_PULSES_PER_MM_X,
                             1.0 / STEPPER_PULSES_PER_MM_Y,
                             1.0 / STEPPER_PULSES_PER_MM_Z,
@@ -122,6 +128,7 @@ class GMachine(object):
 
         logging.info("Moving linearly {}".format(delta))
         gen = PulseGeneratorLinear(delta, velocity)
+        logging.debug("gen: {}".format(gen))
         self.__check_velocity(gen.max_velocity())
         hal.move(gen)
         # save position
@@ -372,7 +379,7 @@ class GMachine(object):
             if pause < 0:
                 raise GMachineException("bad delay")
             hal.join()
-            time.sleep(pause)
+            time.sleep(pause / 1000)
         elif c == 'G17':  # XY plane select
             self._plane = PLANE_XY
         elif c == 'G18':  # ZX plane select
@@ -461,6 +468,14 @@ class GMachine(object):
             hal.join()
             p = self.position()
             answer = "X:{} Y:{} Z:{} E:{}".format(p.x, p.y, p.z, p.e)
+        elif c == 'M300': # play sound normaly or control z axis
+            logging.debug("gcode = {}".format(gcode.get('S')))
+            if gcode.get('S') == 30:
+                self._pen(True)
+            elif gcode.get('S') == 50:
+                self._pen(False)
+            else:
+                raise GMachineException("Not supported, use M300 S30 or M300 S50")
         elif c is None:  # command not specified(ie just F was passed)
             pass
         # commands below are added just for compatibility
diff --git a/cnc/hal.py b/cnc/hal.py
index d6c5c34..11d912a 100644
--- a/cnc/hal.py
+++ b/cnc/hal.py
@@ -135,3 +135,5 @@ if 'deinit' not in locals():
     raise NotImplementedError("hal.deinit() not implemented")
 if 'watchdog_feed' not in locals():
     raise NotImplementedError("hal.watchdog_feed() not implemented")
+if ENABLE_L293D and 'pen_control' not in locals():
+    raise NotImplementedError("hal.pen_control() not implemented")
diff --git a/cnc/hal_raspberry/hal2.py b/cnc/hal_raspberry/hal2.py
index c3fe438..f82d22a 100644
--- a/cnc/hal_raspberry/hal2.py
+++ b/cnc/hal_raspberry/hal2.py
@@ -169,6 +169,9 @@ def init():
         stepper_y.set_dir(stepper.STATE_DIR_INV)
     stepper_y.enable()
     stepper_y.debug()
+    # Init pen pin
+    gpio.init(PEN_PIN, rpgpio.GPIO.MODE_OUTPUT)
+    gpio.clear(PEN_PIN)
     # Watchdog start
     watchdog.start()
 
@@ -186,6 +189,22 @@ def fan_control(on_off):
     """
     logging.debug("fan control not implemented")
 
+def pen_control(up_down):
+    """
+    Pen control.
+    :param on_off: boolean value if pen is up or down.
+    """
+    if up_down:
+        logging.info("Pen is up ...")
+        pwm.add_pin(PEN_PIN, 5)
+        time.sleep(0.25)
+        pwm.add_pin(PEN_PIN, 0)
+    else:
+        logging.info("Pen is down ...")
+        pwm.add_pin(PEN_PIN, 12.5)
+        time.sleep(0.25)
+        pwm.add_pin(PEN_PIN, 0)
+
 def extruder_heater_control(percent):
     """ Extruder heater control.
     :param percent: heater power in percent 0..100. 0 turns heater off.
@@ -346,6 +365,7 @@ def deinit():
     join()
     disable_steppers()
     pwm.remove_all()
+    gpio.clear(PEN_PIN)
     watchdog.stop()
 
 def watchdog_feed():
diff --git a/cnc/hal_raspberry/rpgpio.py b/cnc/hal_raspberry/rpgpio.py
index 87455d4..9dad98b 100755
--- a/cnc/hal_raspberry/rpgpio.py
+++ b/cnc/hal_raspberry/rpgpio.py
@@ -260,7 +260,8 @@ class DMAGPIO(DMAProto):
 class DMAPWM(DMAProto):
     _DMA_CONTROL_BLOCK_SIZE = 32
     _DMA_DATA_OFFSET = 24
-    _TOTAL_NUMBER_OF_BLOCKS = 256
+    #_TOTAL_NUMBER_OF_BLOCKS = 256
+    _TOTAL_NUMBER_OF_BLOCKS = 76500 
     _DMA_CHANNEL = 14
 
     def __init__(self):
diff --git a/cnc/main.py b/cnc/main.py
index ebcc6f6..919d8c5 100755
--- a/cnc/main.py
+++ b/cnc/main.py
@@ -42,8 +42,8 @@ def do_line(line):
 
 
 def main():
-    #logging_config.debug_disable()
-    logging_config.debug_enable()
+    logging_config.debug_disable()
+    #logging_config.debug_enable()
     try:
         if len(sys.argv) > 1:
             # Read file with gcode
diff --git a/tests/rects.gcode b/tests/rects.gcode
index 0c03db8..ff4380f 100644
--- a/tests/rects.gcode
+++ b/tests/rects.gcode
@@ -1,62 +1,92 @@
 G91 f2000
 ; run
+M300 S30
 G1 x39 f2000
 G1 y49 f2000
 G1 x-39 f2000
 G1 y-49 f2000
+M300 S50
+M300 S30
 G1 x35 f2500
 G1 y45 f2500
 G1 x-35 f2500
 G1 y-45 f2500
+M300 S50
+M300 S30
 G1 x30 f3000
 G1 y40 f3000
 G1 x-30 f3000
 G1 y-40 f3000
+M300 S50
+M300 S30
 G1 x20 f3500
 G1 y35 f3500
 G1 x-20 f3500
 G1 y-35 f3500
+M300 S50
+M300 S30
 G1 x10 f4000
 G1 y45 f4000
 G1 x-10 f4000
 G1 y-45 f4000
+M300 S50
+M300 S30
 G1 x39 f2000
 G1 y49 f2000
 G1 x-39 f2000
 G1 y-49 f2000
+M300 S50
+M300 S30
 G1 x35 f2500
 G1 y45 f2500
 G1 x-35 f2500
 G1 y-45 f2500
+M300 S50
+M300 S30
 G1 x30 f3000
 G1 y40 f3000
 G1 x-30 f3000
 G1 y-40 f3000
+M300 S50
+M300 S30
 G1 x20 f3500
 G1 y35 f3500
 G1 x-20 f3500
 G1 y-35 f3500
+M300 S50
+M300 S30
 G1 x10 f4000
 G1 y45 f4000
 G1 x-10 f4000
 G1 y-45 f4000
+M300 S50
+M300 S30
 G1 x39 f2000
 G1 y49 f2000
 G1 x-39 f2000
 G1 y-49 f2000
+M300 S50
+M300 S30
 G1 x35 f2500
 G1 y45 f2500
 G1 x-35 f2500
 G1 y-45 f2500
+M300 S50
+M300 S30
 G1 x30 f3000
 G1 y40 f3000
 G1 x-30 f3000
 G1 y-40 f3000
+M300 S50
+M300 S30
 G1 x20 f3500
 G1 y35 f3500
 G1 x-20 f3500
 G1 y-35 f3500
+M300 S50
+M300 S30
 G1 x10 f4000
 G1 y45 f4000
 G1 x-10 f4000
 G1 y-45 f4000
+M300 S50