diff --git a/cnc/config.py b/cnc/config.py
index 81eb1c1..4231f7c 100644
--- a/cnc/config.py
+++ b/cnc/config.py
@@ -31,8 +31,8 @@ STEPPER_STEPS_PER_REV_Y = 20
 
 # Invert axises direction, by default(False) high level means increase of
 # position. For inverted(True) axis, high level means decrease of position.
-STEPPER_INVERTED_X = True
-STEPPER_INVERTED_Y = False
+STEPPER_INVERTED_X = False 
+STEPPER_INVERTED_Y = True
 STEPPER_INVERTED_Z = False
 STEPPER_INVERTED_E = True
 
@@ -44,8 +44,8 @@ ENDSTOP_INVERTED_Y = True
 ENDSTOP_INVERTED_Z = False  # Auto leveler
 
 # Workplace physical size.
-TABLE_SIZE_X_MM = 40
-TABLE_SIZE_Y_MM = 40
+TABLE_SIZE_X_MM = 41
+TABLE_SIZE_Y_MM = 51
 TABLE_SIZE_Z_MM = 0
 
 # Mixed settings.
diff --git a/cnc/hal_raspberry/hal2.py b/cnc/hal_raspberry/hal2.py
index b549a8b..c3fe438 100644
--- a/cnc/hal_raspberry/hal2.py
+++ b/cnc/hal_raspberry/hal2.py
@@ -46,7 +46,7 @@ class stepper:
         self.state = self.STEPPER_DISABLED 
         self.dir = self.STATE_DIR_DIRECT
         self.axe = axe
-        self.seq = self.FULL_STEP_SEQ
+        self.seq = self.FULL_STEP_SEQ[:]
         self.pins = pins
         self.step_number = 0
         self.current_seq_num = 0 
@@ -68,11 +68,11 @@ class stepper:
         if mode > self.MODE_HALF:
             raise ValueError
         elif mode == self.MODE_FULL:
-            self.seq = self.FULL_STEP_SEQ
+            self.seq = self.FULL_STEP_SEQ[:]
             if self.dir == self.STATE_DIR_INV:
                 self.seq.reverse()
         elif mode == self.MODE_HALF:
-            self.seq = self.HALF_STEP_SEQ
+            self.seq = self.HALF_STEP_SEQ[:]
             if self.dir == self.STATE_DIR_INV:
                 self.seq.reverse()
 
@@ -81,26 +81,41 @@ class stepper:
         if direction > self.STATE_DIR_INV:
             raise ValueError
         elif direction == self.STATE_DIR_INV and self.dir == self.STATE_DIR_DIRECT:
+            #logging.debug("SET DIR INV")
             self.dir = direction
+            #logging.debug("BEFORE: {}".format(self.seq))
             self.seq.reverse()
+            #logging.debug("AFTER: {}".format(self.seq))
+            if self.current_seq_num > 0:
+                self.current_seq_num = len(self.seq) - 1 - self.current_seq_num
         elif direction == self.STATE_DIR_DIRECT and self.dir == self.STATE_DIR_INV:
+            #logging.debug("SET DIR DIRECT")
             self.dir = direction
+            #logging.debug("BEFORE: {}".format(self.seq))
             self.seq.reverse()
+            #logging.debug("AFTER: {}".format(self.seq))
+            if self.current_seq_num > 0:
+                self.current_seq_num = len(self.seq) - 1 - self.current_seq_num
         else:
             logging.warning("direction already set")
 
     def inc_seq_num(self):
         ''' increment current sequence number '''
-        self.current_seq_num += 1
+        if self.current_seq_num >= len(self.seq) - 1:
+            self.current_seq_num = 0
+        else:
+            self.current_seq_num += 1
 
     def dec_seq_num(self):
         ''' decrement current sequence number '''
-        self.current_seq_num -= 1
+        if self.current_seq_num == 0:
+            self.current_seq_num = len(self.seq) - 1
+        else:
+            self.current_seq_num -= 1
 
     def get_current_seq(self):
         ''' get current sequence '''
-        i = self.current_seq_num % (len(self.seq))
-        return self.seq[i]
+        return self.seq[self.current_seq_num]
 
     def disable(self):
         ''' disable stepper '''
@@ -112,9 +127,24 @@ class stepper:
         ''' enable stepper '''
         self.state = self.STEPPER_ENABLED
 
-    def set_speed(self, speed):
-        ''' sets the speed in rev per minute '''
-        logging.debug("speed = {}".format(60*1000*1000 / self.number_of_steps / speed))
+    def debug(self):
+        ''' debug mode '''
+        logging.debug("state = {} - \
+dir = {} - \
+axe = {} - \
+seq = {} - \
+pins = {} - \
+step_number = {} - \
+current_seq_num = {} - \
+number_of_steps = {}".format(self.state, 
+                             self.dir,
+                             self.axe,
+                             self.seq,
+                             self.pins,
+                             self.step_number,
+                             self.current_seq_num,
+                             self.number_of_steps))
+
 
 stepper_x = stepper(axe=stepper.AXE_X, pins=STEPPER_STEP_PINS_X)
 stepper_y = stepper(axe=stepper.AXE_Y, pins=STEPPER_STEP_PINS_Y)
@@ -127,12 +157,18 @@ def init():
     stepper_x.init()
     stepper_x.set_mode(mode = stepper.MODE_HALF)
     stepper_x.set_steps_number(STEPPER_STEPS_PER_REV_X)
+    if STEPPER_INVERTED_X:
+        stepper_x.set_dir(stepper.STATE_DIR_INV)
     stepper_x.enable()
+    stepper_x.debug()
     # Init Y stepper
-#    stepper_y.init()
-#    stepper_y.set_mode(mode = stepper.MODE_HALF)
-#    stepper_x.set_steps_number(STEPPER_STEPS_PER_REV_Y)
-#    stepper_y.enable()
+    stepper_y.init()
+    stepper_y.set_mode(mode = stepper.MODE_HALF)
+    stepper_y.set_steps_number(STEPPER_STEPS_PER_REV_Y)
+    if STEPPER_INVERTED_Y:
+        stepper_y.set_dir(stepper.STATE_DIR_INV)
+    stepper_y.enable()
+    stepper_y.debug()
     # Watchdog start
     watchdog.start()
 
@@ -217,26 +253,30 @@ def move(generator):
     idx = 0
     for direction, tx, ty, tz, te in generator:
         if current_cb is not None:
-            logging.debug("{} + {} => result = {}".format(dma.current_address(), bytes_per_iter, dma.current_address() + bytes_per_iter))
+            #logging.debug("{} + {} => result = {}".format(dma.current_address(), bytes_per_iter, dma.current_address() + bytes_per_iter))
             while dma.current_address() + bytes_per_iter >= current_cb:
                 time.sleep(0.001)
                 current_cb = dma.current_control_block()
-                logging.debug("current control block : {}".format(current_cb))
+                #logging.debug("current control block : {}".format(current_cb))
                 if current_cb is None:
                     k0 = k
                     st = time.time()
                     break  # previous dma sequence has stopped
 #        logging.debug("[{}] direction: {} - tx: {} - ty: {} - tz: {} - te: {}".format(idx, direction, tx, ty, tz, te))
         if direction:  # set up directions
+            logging.debug("[{}] direction: {} - tx: {} - ty: {} - tz: {} - te: {}".format(idx, direction, tx, ty, tz, te))
             if tx > 0:
+                logging.debug("TX Direct")
                 stepper_x.set_dir(stepper.STATE_DIR_DIRECT)
             elif tx < 0:
+                logging.debug("TX Inverse")
                 stepper_x.set_dir(stepper.STATE_DIR_INV)
             if ty > 0:
+                logging.debug("TY Direct")
                 stepper_y.set_dir(stepper.STATE_DIR_DIRECT)
             elif ty < 0:
+                logging.debug("TY Inverse")
                 stepper_y.set_dir(stepper.STATE_DIR_INV)
-            dma.add_delay(STEPPER_PULSE_LENGTH_US)
             continue
         
         mask_x = 0
@@ -248,26 +288,25 @@ def move(generator):
             cur_seq_x = stepper_x.get_current_seq()
             #logging.debug("[TX] prev = {} - K = {} - diff : {} - cur_seq_x : {}".format(prevx, kx, kx - prevx, cur_seq_x)) 
             # mask pins
-            for i, enable in enumerate(cur_seq_x):
-                if enable:
+            for i, enable_x in enumerate(cur_seq_x):
+                if enable_x:
                     mask_x += 1 << STEPPER_STEP_PINS_X[i]
             # set pulse with diff between current time and previous time
-#            logging.debug("[TX] MASK: {} - TIME(us): {}".format(bin(mask_x), kx - prevx))
+            #logging.debug("[TX] MASK: {} - TIME(us): {}".format(bin(mask_x), kx - prevx))
             dma.add_pulse(mask_x, kx - prevx)
             stepper_x.inc_seq_num()
             prevx = kx + STEPPER_PULSE_LENGTH_US
         if ty is not None:
-            logging.debug("not implemented ...")
-#            ky = int(round(ty * US_IN_SECONDS))
-#            cur_seq_y = stepper_y.get_current_seq()
-#            #logging.debug("[TY] prev = {} - K = {} - diff : {} - cur_seq_y : {}".format(prevy, ky, ky - prevy, cur_seq_y)) 
-#            for j, enable in enumerate(cur_seq_y):
-#                if enable:
-#                    mask_y += 1 << STEPPER_STEP_PINS_Y[j]
-#            logging.debug("[TY] MASK: {} - TIME(us): {}".format(bin(mask_y), ky - prevy))
-#            dma.add_pulse(mask_y, ky - prevy)
-#            stepper_y.inc_seq_num()
-#            prevy = ky + STEPPER_PULSE_LENGTH_US
+            ky = int(round(ty * US_IN_SECONDS))
+            cur_seq_y = stepper_y.get_current_seq()
+            #logging.debug("[TY] prev = {} - K = {} - diff : {} - cur_seq_y : {}".format(prevy, ky, ky - prevy, cur_seq_y)) 
+            for j, enable_y in enumerate(cur_seq_y):
+                if enable_y:
+                    mask_y += 1 << STEPPER_STEP_PINS_Y[j]
+            #logging.debug("[TY] MASK: {} - TIME(us): {}".format(bin(mask_y), ky - prevy))
+            dma.add_pulse(mask_y, ky - prevy)
+            stepper_y.inc_seq_num()
+            prevy = ky + STEPPER_PULSE_LENGTH_US
         # run stream ...
 #        if not is_ran and instant and current_cb is None:
 #            # TODO: wait 100 ms
@@ -282,7 +321,6 @@ def move(generator):
         # after long command, we can fill short buffer, that why we may need to
         #  wait until long command finishes
         while dma.is_active():
-            logging.debug("wait ...")
             time.sleep(0.01)
         dma.run(False)
     else:
diff --git a/cnc/hal_raspberry/rpgpio.py b/cnc/hal_raspberry/rpgpio.py
index 44abdd0..87455d4 100755
--- a/cnc/hal_raspberry/rpgpio.py
+++ b/cnc/hal_raspberry/rpgpio.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 
-from .rpgpio_private import *
-#from rpgpio_private import *
+#from .rpgpio_private import *
+from rpgpio_private import *
 
 import time
 import logging
@@ -463,9 +463,9 @@ def test():
 
 # for testing purpose
 def main():
-    #pins_x = [4,17,27,22]
-    pins_x = [22,27,17,4]
-    pins_y = [12,16,20,21]
+    #pins_x = [22,27,17,4]
+    #pins_y = [12,16,20,21]
+    pins_x = [12,16,20,21]
     seqs = [[1,0,0,0], # Phase A
             [1,0,1,0], # Phase AB
             [0,0,1,0], # Phase B
@@ -480,44 +480,38 @@ def main():
         print("Start output PIN: {}".format(pin))
         g.init(pin, GPIO.MODE_OUTPUT)
 
-    #for pin in pins_y:
-    #    print("Start output PIN: {}".format(pin))
-    #    g.init(pin, GPIO.MODE_OUTPUT)
     dg = DMAGPIO()
+    dg.clear()
 
-#    counter_max = 100
-#    counter_current = 0
-#    while counter_current < counter_max:
-#        for i in range(0, 63):
-#            for seq in seqs:
-#                mask = 0
-#                for idx, enable in enumerate(seq):
-#                    if enable:
-#                        mask += 1 << pins_x[idx]
-#                        #mask += 1 << pins_y[idx]
-#                dg.add_pulse(mask, 1000)
-#        dg.finalize_stream()
-#
-#        dg.run_stream()
-#        while dg.is_active():
-#            time.sleep(0.01)
-#        dg.clear()
-#        seqs.reverse()
-#        counter_current += 1
-    try:
-        print("press enter to stop...")
-        sys.stdin.readline()
-    except KeyboardInterrupt:
-        pass
+    counter_max = 100
+    counter_current = 0
+    while counter_current < counter_max:
+        for i in range(0, 63):
+            for seq in seqs:
+                mask = 0
+                for idx, enable in enumerate(seq):
+                    if enable:
+                        mask += 1 << pins_x[idx]
+                dg.add_pulse(mask, 1000)
+        dg.finalize_stream()
+
+        dg.run_stream()
+        while dg.is_active():
+            time.sleep(0.01)
+        dg.clear()
+        seqs.reverse()
+        counter_current += 1
+#    try:
+#        print("press enter to stop...")
+#        sys.stdin.readline()
+#    except KeyboardInterrupt:
+#        pass
     dg.clear()
     dg.stop()
 
     for pin in pins_x:
         print("Stop output PIN: {}".format(pin))
         g.clear(pin)
-#    for pin in pins_y:
-#        print("Stop output PIN: {}".format(pin))
-#        g.clear(pin)
 
 if __name__ == "__main__":
     main()
diff --git a/tests/rects.gcode b/tests/rects.gcode
index fe5e2b1..0c03db8 100644
--- a/tests/rects.gcode
+++ b/tests/rects.gcode
@@ -1,36 +1,62 @@
-g21
-g90
-; move to start position
-g1x50y50f1800
-z20f120
-f1800
-g91
+G91 f2000
 ; run
-x100
-y100
-x-100
-y-90
-x90
-y80
-x-80
-y-70
-x70
-y60
-x-60
-y-50
-x50
-y40
-x-40
-y-30
-x30
-y20
-x-20
-y-10
-x10
-y5
-x-5
-y-2.5
-x2.5
-; homing
-g28
-
+G1 x39 f2000
+G1 y49 f2000
+G1 x-39 f2000
+G1 y-49 f2000
+G1 x35 f2500
+G1 y45 f2500
+G1 x-35 f2500
+G1 y-45 f2500
+G1 x30 f3000
+G1 y40 f3000
+G1 x-30 f3000
+G1 y-40 f3000
+G1 x20 f3500
+G1 y35 f3500
+G1 x-20 f3500
+G1 y-35 f3500
+G1 x10 f4000
+G1 y45 f4000
+G1 x-10 f4000
+G1 y-45 f4000
+G1 x39 f2000
+G1 y49 f2000
+G1 x-39 f2000
+G1 y-49 f2000
+G1 x35 f2500
+G1 y45 f2500
+G1 x-35 f2500
+G1 y-45 f2500
+G1 x30 f3000
+G1 y40 f3000
+G1 x-30 f3000
+G1 y-40 f3000
+G1 x20 f3500
+G1 y35 f3500
+G1 x-20 f3500
+G1 y-35 f3500
+G1 x10 f4000
+G1 y45 f4000
+G1 x-10 f4000
+G1 y-45 f4000
+G1 x39 f2000
+G1 y49 f2000
+G1 x-39 f2000
+G1 y-49 f2000
+G1 x35 f2500
+G1 y45 f2500
+G1 x-35 f2500
+G1 y-45 f2500
+G1 x30 f3000
+G1 y40 f3000
+G1 x-30 f3000
+G1 y-40 f3000
+G1 x20 f3500
+G1 y35 f3500
+G1 x-20 f3500
+G1 y-35 f3500
+G1 x10 f4000
+G1 y45 f4000
+G1 x-10 f4000
+G1 y-45 f4000