diff --git a/turtles/mine.lua b/turtles/mine.lua
index 39c3ebf..639b690 100644
--- a/turtles/mine.lua
+++ b/turtles/mine.lua
@@ -51,15 +51,49 @@ function turn_rel(dir)
     turn_abs((facing + dir) % 4)
 end
 
+-- Dumps the current turtle position
+function _dump_pos()
+    print("Current position: H "..height.." S "..side.." D "..depth)
+end
+
+-- Updates the turtle position for a move in the given direction
+function _count_move_dir(dir)
+    dir = dir % 4
+    if      dir == 0 then depth = depth + 1
+    elseif  dir == 1 then side = side + 1
+    elseif  dir == 2 then depth = depth - 1
+    elseif  dir == 3 then side = side - 1
+    end
+end
+
+function move_forward()
+    rc, desc = turtle.forward()
+    if not rc then abort("Move: "..desc) end
+    _count_move_dir(facing)
+end
+function move_back()
+    rc, desc = turtle.back()
+    if not rc then abort("Move: "..desc) end
+    _count_move_dir(facing + 2)
+end
+function move_up()
+    rc, desc = turtle.up()
+    if not rc then abort("Move: "..desc) end
+    height = height + 1
+end
+function move_down()
+    rc, desc = turtle.down()
+    if not rc then abort("Move: "..desc) end
+    height = height - 1
+end
+
 function go_to_depth(dd)
     turn_abs(0)
     while depth > dd do
-        turtle.back()
-        depth = depth - 1
+        move_back()
     end
     while depth < dd do
-        turtle.forward()
-        depth = depth + 1
+        move_forward()
     end
 end
 
@@ -68,24 +102,24 @@ end
 function go_to(dh, dd, ds)
     go_to_depth(0)
 
-    while height > dh do
-        turtle.down()
-        height = height - 1
-    end
-    while height < dh do
-        turtle.up()
-        height = height + 1
+    if height ~= dh then
+        while height > dh do
+            move_down()
+        end
+        while height < dh do
+            move_up()
+        end
     end
 
-    turn_abs(1)
+    if side ~= ds then
+        turn_abs(1)
 
-    while side > ds do
-        turtle.back()
-        side = side - 1
-    end
-    while side < ds do
-        turtle.forward()
-        side = side + 1
+        while side > ds do
+            move_back()
+        end
+        while side < ds do
+            move_forward()
+        end
     end
 
     go_to_depth(dd)
@@ -134,9 +168,8 @@ function find_next_shaft()
     go_to(0, 0, 0)
 
     while true do
-        print("Climb")
-        for hei=0,MAX_HEIGHT-1 do
-            if (hei % 3) == 0 then
+        for hei=1,MAX_HEIGHT-1 do
+            if (height % 3) == 0 then
                 if turtle.detect() then
                     return
                 end
@@ -145,8 +178,10 @@ function find_next_shaft()
             if turtle.detectUp() then
                 turtle.digUp()
             end
-            if not turtle.up() then abort("Misshaped lobby") end
-            height = height + 1
+            move_up()
+        end
+        if turtle.detect() then
+            return
         end
 
         turn_abs(1)
@@ -154,14 +189,12 @@ function find_next_shaft()
             if turtle.detect() then
                 turtle.dig()
             end
-            if not turtle.forward() then abort("Misshaped lobby") end
-            side = side + 1
+            move_forward()
         end
         turn_abs(0)
 
-        print("Down")
-        for hei=MAX_HEIGHT-1,0,-1 do
-            if (hei % 3) ~= 0 then
+        for hei=MAX_HEIGHT-1,1,-1 do
+            if (height % 3) ~= 0 then
                 if turtle.detect() then
                     return
                 end
@@ -170,8 +203,7 @@ function find_next_shaft()
             if turtle.detectDown() then
                 turtle.digDown()
             end
-            if not turtle.down() then abort("Misshaped lobby") end
-            height = height - 1
+            move_down()
         end
 
         turn_abs(1)
@@ -179,8 +211,7 @@ function find_next_shaft()
             if turtle.detect() then
                 turtle.dig()
             end
-            if not turtle.forward() then abort("Misshaped lobby") end
-            side = side + 1
+            move_forward()
         end
         turn_abs(0)
     end
@@ -203,7 +234,8 @@ function main()
     while true do
         print("Finding shaft")
         find_next_shaft()
-        print("Mining shaft")
+        print("Mining shaft at position:")
+        _dump_pos()
         mine_shaft()
 
         print("Discharging")