diff --git a/controller/testing_controller.lua b/controller/testing_controller.lua
index 837103c..e17828a 100644
--- a/controller/testing_controller.lua
+++ b/controller/testing_controller.lua
@@ -26,21 +26,24 @@ end
 function TestingController:TestRelativeMovementWithRelativeRotation()
     print("Testing relative movement with relative rotation...")
 
-                                     -- N: BotCenter
-    self.controller:MoveRelative(3)
-    self.controller:TurnRelative(1)  -- E: TopCenter
-    self.controller:MoveRelative(3)
-    self.controller:TurnRelative(2)  -- W: TopRight
-    self.controller:MoveRelative(3)
-    self.controller:TurnRelative(10) -- E: TopCenter
-    self.controller:MoveRelative(3)
-    self.controller:TurnRelative(-3) -- S: TopRight
-    self.controller:MoveRelative(3)
-    self.controller:TurnRelative(1)  -- W: BotRight
-    self.controller:MoveRelative(3)
-    self.controller:TurnRelative(1)  -- N: BotCenter
+                                     -- N: BackCenter (Center)
+    self.controller:MoveForward(3)
+    self.controller:TurnRelative(1)  -- E: FrontCenter (Center)
+    self.controller:MoveForward(3)
+    self.controller:TurnRelative(2)  -- W: FrontRight (Center)
+    self.controller:MoveVertical(3)
+    self.controller:TurnRelative(8)  -- W: FrontRight (Top)
+    self.controller:MoveForward(3)
+    self.controller:TurnRelative(3)  -- S: FrontCenter (Top)
+    self.controller:MoveForward(3)
+    self.controller:TurnRelative(9)  -- W: BackCenter (Top)
+    self.controller:MoveVertical(-3)
+    self.controller:TurnRelative(1)  -- N: BackCenter (Center)
 
     print("The turtle should be in its original location.")
+    print(("The turtle has internal position (%d, %d, %d) and internal direction %d"):format(
+        self.controller.position.x, self.controller.position.y, self.controller.position.z, self.controller.position.dir
+    ))
     print("Press Enter to continue")
     _ = io.read()
 end
@@ -49,21 +52,24 @@ end
 function TestingController:TestRelativeMovementWithAbsoluteRotation()
     print("Testing relative movement with absolute rotation...")
 
-                                                     -- N: BotCenter
-    self.controller:MoveRelative(3)
-    self.controller:TurnToDirection(Direction.EAST)  -- E: TopCenter
-    self.controller:MoveRelative(3)
-    self.controller:TurnToDirection(Direction.WEST)  -- W: TopRight
-    self.controller:MoveRelative(3)
-    self.controller:TurnToDirection(Direction.EAST)  -- E: TopCenter
-    self.controller:MoveRelative(3)
-    self.controller:TurnToDirection(Direction.SOUTH) -- S: TopRight
-    self.controller:MoveRelative(3)
-    self.controller:TurnToDirection(Direction.WEST)  -- W: BotRight
-    self.controller:MoveRelative(3)
-    self.controller:TurnToDirection(Direction.NORTH) -- N: BotCenter
+                                                     -- N: BackCenter (Center)
+    self.controller:MoveForward(3)
+    self.controller:TurnToDirection(Direction.EAST)  -- E: FrontCenter (Center)
+    self.controller:MoveForward(3)
+    self.controller:TurnToDirection(Direction.WEST)  -- W: FrontRight (Center)
+    self.controller:MoveVertical(3)
+    self.controller:TurnToDirection(Direction.WEST)  -- W: FrontRight (Top)
+    self.controller:MoveForward(3)
+    self.controller:TurnToDirection(Direction.SOUTH) -- S: FrontCenter (Top)
+    self.controller:MoveForward(3)
+    self.controller:TurnToDirection(Direction.WEST)  -- W: BackCenter (Top)
+    self.controller:MoveVertical(-3)
+    self.controller:TurnToDirection(Direction.NORTH) -- N: BackCenter (Center)
 
     print("The turtle should be in its original location.")
+    print(("The turtle has internal position (%d, %d, %d) and internal direction %d"):format(
+        self.controller.position.x, self.controller.position.y, self.controller.position.z, self.controller.position.dir
+    ))
     print("Press Enter to continue")
     _ = io.read()
 end
@@ -72,20 +78,18 @@ end
 function TestingController:TestAbsoluteMovementWithoutStack()
     print("Testing absolute movement without stack...")
 
-                                                             -- N: BotCenter
-    self.controller:MoveToPosition(0, 0, 3, Direction.EAST)  -- E: TopCenter
-    _ = io.read()
-    self.controller:MoveToPosition(3, 0, 3, Direction.WEST)  -- W: TopRight
-    _ = io.read()
-    self.controller:MoveToPosition(0, 0, 3, Direction.EAST)  -- E: TopCenter
-    _ = io.read()
-    self.controller:MoveToPosition(3, 0, 3, Direction.SOUTH) -- S: TopRight
-    _ = io.read()
-    self.controller:MoveToPosition(3, 0, 0, Direction.WEST)  -- W: BotRight
-    _ = io.read()
-    self.controller:MoveToPosition(0, 0, 0, Direction.NORTH) -- N: BotCenter
+                                                             -- N: BackCenter (Center)
+    self.controller:MoveToPosition(0, 0, 3, Direction.EAST)  -- E: FrontCenter (Center)
+    self.controller:MoveToPosition(3, 0, 3, Direction.WEST)  -- W: FrontRight (Center)
+    self.controller:MoveToPosition(3, 3, 3, Direction.WEST)  -- W: FrontRight (Top)
+    self.controller:MoveToPosition(0, 3, 3, Direction.SOUTH) -- S: FrontCenter (Top)
+    self.controller:MoveToPosition(0, 3, 0, Direction.WEST)  -- W: BackCenter (Top)
+    self.controller:MoveToPosition(0, 0, 0, Direction.NORTH) -- N: BackCenter (Center)
 
     print("The turtle should be in its original location.")
+    print(("The turtle has internal position (%d, %d, %d) and internal direction %d"):format(
+        self.controller.position.x, self.controller.position.y, self.controller.position.z, self.controller.position.dir
+    ))
     print("Press Enter to continue")
     _ = io.read()
 end
@@ -94,16 +98,16 @@ end
 function TestingController:TestAbsoluteMovementWithStack()
     print("Testing absolute movement with stack...")
 
-                                                            -- N: BotCenter
-    self.controller:MoveToPosition(0, 0, 3, Direction.EAST) -- E: TopCenter
-    _ = io.read()
-    self.controller:MoveBack()                              -- N: BotCenter
-    _ = io.read()
-    self.controller:MoveToPosition(3, 0, 3, Direction.WEST) -- W: TopRight
-    _ = io.read()
-    self.controller:MoveBack()                              -- N: BotCenter
+                                                            -- N: BotCenter (Center)
+    self.controller:MoveToPosition(0, 0, 3, Direction.EAST) -- E: TopCenter (Center)
+    self.controller:MoveBack()                              -- N: BotCenter (Center)
+    self.controller:MoveToPosition(3, 3, 3, Direction.WEST) -- W: TopRight (Top)
+    self.controller:MoveBack()                              -- N: BotCenter (Center)
 
     print("The turtle should be in its original location.")
+    print(("The turtle has internal position (%d, %d, %d) and internal direction %d"):format(
+        self.controller.position.x, self.controller.position.y, self.controller.position.z, self.controller.position.dir
+    ))
     print("Press Enter to continue")
     _ = io.read()
 end
@@ -111,12 +115,13 @@ end
 
 function TestingController:TestUnstocking()
     print("Testing inventory unloading...")
-    print("Please fill multiple inventory slots with items.")
-    _ = io.read()
 
     self.controller:MoveToPosition(3, 0, 3, Direction.WEST)
+    print("Please fill the entire inventory with items.")
+    print("Press Enter to continue")
     _ = io.read()
     self.controller:UnstockIfFull(true)
+    print("Press Enter to continue")
     _ = io.read()
     self.controller:MoveBack()
 
@@ -128,12 +133,13 @@ end
 
 function TestingController:TestRefueling()
     print("Testing refueling...")
-    print("Please fill multiple inventory slots with items.")
-    _ = io.read()
 
     self.controller:MoveToPosition(3, 0, 3, Direction.WEST)
+    print("Please fill multiple inventory slots with items.")
+    print("Press Enter to continue")
     _ = io.read()
     self.controller:RefuelIfEmpty(true)
+    print("Press Enter to continue")
     _ = io.read()
     self.controller:MoveBack()
 
@@ -150,7 +156,7 @@ end
 
 function TestingController:Run()
     self.controller:Configure()
-    self.controller:DisableMining()
+    self.controller:DisableMiningForward()
 
     print("There are multiple tests available:")
     print("1: Relative movement with relative rotation")
diff --git a/controller/turtle_controller.lua b/controller/turtle_controller.lua
index 8956760..e2dc90c 100644
--- a/controller/turtle_controller.lua
+++ b/controller/turtle_controller.lua
@@ -8,15 +8,13 @@ local Stack = require("lib.stack")
 ---@field config TurtleControllerConfig
 ---@field position Position
 ---@field last_positions Stack
----@field mine boolean
+---@field mine_forward boolean
+---@field mine_above boolean
+---@field mine_below boolean
 local TurtleController = {}
 TurtleController.__index = TurtleController
 
 
--- TODO: StackOverflow once the inventory is full...
--- TODO: Test if there's a chest when dropping/sucking/mining (don't mine chests!)
-
-
 ---@return TurtleController
 function TurtleController:Create()
     local t = {}
@@ -37,7 +35,9 @@ function TurtleController:Create()
     }
     t.position = Position:Empty()
     t.last_positions = Stack:Create()
-    t.mine = false
+    t.mine_forward = false
+    t.mine_above = false
+    t.mine_below = false
 
 
     return t
@@ -52,21 +52,19 @@ end
 ---Positive numbers turn clockwise, negative numbers turn counterclockwise
 ---@param number_of_turns number
 function TurtleController:TurnRelative(number_of_turns)
-    if number_of_turns == 0 then
+    if number_of_turns % 4 == 0 then
         return
     end
 
-    self:RefuelIfEmpty()
-
     -- Turn turtle
     local turns = number_of_turns % 4
     if turns == 3 then
-        print(("Turtle is turning by %d (shortened to %d)..."):format(number_of_turns, -1))
+        -- print(("Turtle is turning by %d (shortened to %d)..."):format(number_of_turns, -1))
 
         -- If we're rotating by 3, we could turn faster by rotating by 1 in the opposite direction
         turtle.turnLeft()
     else
-        print(("Turtle is turning by %d (shortened to %d)..."):format(number_of_turns, turns))
+        -- print(("Turtle is turning by %d (shortened to %d)..."):format(number_of_turns, turns))
 
         for _ = 1,turns do
             turtle.turnRight()
@@ -90,31 +88,42 @@ end
 
 ---Move forward by a number of blocks depending on the current rotation
 ---@param number_of_blocks number
-function TurtleController:MoveRelative(number_of_blocks)
+---@param skip_unstocking boolean | nil
+---@param skip_refueling boolean | nil
+function TurtleController:MoveForward(number_of_blocks, skip_unstocking, skip_refueling)
+    skip_unstocking = skip_unstocking or false
+    skip_refueling = skip_refueling or false
     if number_of_blocks == 0 then
         return
     end
 
-    print(("Turtle is moving forward by %d blocks..."):format(number_of_blocks))
+    -- print(("Turtle is moving forward by %d blocks..."):format(number_of_blocks))
 
     for _ = 1,math.abs(number_of_blocks) do
-        self:RefuelIfEmpty()
-        self:UnstockIfFull()
-
-        -- Break blocks
-        if self.mine then
-            while not turtle.forward() do
-                turtle.dig()
-            end
-            turtle.digUp()
-            turtle.digDown()
-        else
-            if not turtle.forward() then
-                printError("Turtle wants to move without mining but is blocked!")
-            end
+        if not skip_refueling then
+            self:RefuelIfEmpty()
+        end
+        if not skip_unstocking then
+            self:UnstockIfFull()
         end
 
-        -- Update current_position
+        -- Mine
+        if self.mine_forward then
+            turtle.dig()
+        end
+        if self.mine_above then
+            turtle.digUp()
+        end
+        if self.mine_below then
+            turtle.digDown()
+        end
+
+        -- Move
+        if not turtle.forward() then
+            error("Turtle failed to move forward!")
+        end
+
+        -- Update current position
         if self.position.dir == Direction.NORTH then
             self.position.z = self.position.z + 1
         elseif self.position.dir == Direction.SOUTH then
@@ -127,35 +136,71 @@ function TurtleController:MoveRelative(number_of_blocks)
     end
 end
 
+---Positive numbers move up, negative numbers move down.
+---@param number_of_blocks number
+---@param skip_unstocking boolean | nil
+---@param skip_refueling boolean | nil
+function TurtleController:MoveVertical(number_of_blocks, skip_unstocking, skip_refueling)
+    skip_unstocking = skip_unstocking or false
+    skip_refueling = skip_refueling or false
+    if number_of_blocks == 0 then
+        return
+    end
 
+    -- print(("Turtle is moving vertically by %d blocks..."):format(number_of_blocks))
+
+    local move_function = number_of_blocks > 0 and turtle.up or turtle.down
+    local mine_function = number_of_blocks > 0 and turtle.digUp or turtle.digDown
+    local mine_enabled = number_of_blocks > 0 and self.mine_above or self.mine_below
+
+    for _ = 1,math.abs(number_of_blocks) do
+        if not skip_refueling then
+            self:RefuelIfEmpty()
+        end
+        if not skip_unstocking then
+            self:UnstockIfFull()
+        end
+
+        -- Mine
+        if mine_enabled then
+            mine_function()
+        end
+
+        -- Move
+        if not move_function() then
+            error("Turtle failed to move vertically!")
+        end
+
+        -- Update current position
+        if number_of_blocks > 0 then
+            self.position.y = self.position.y + 1
+        elseif number_of_blocks < 0 then
+            self.position.y = self.position.y - 1
+        end
+    end
+end
+
+
+---Move to an absolute position. Stores the current position on the stack so we can return using TurtleController:MoveBack()
 ---@param x number The EAST/WEST axis (grows from WEST to EAST)
 ---@param y number The UP/DOWN axis (grows from DOWN to UP)
 ---@param z number The NORTH/SOUTH axis (grows from SOUTH to NORTH)
 ---@param dir Direction
-function TurtleController:MoveToPosition(x, y, z, dir)
-    print(("Turtle is moving to (%d, %d, %d)..."):format(x, y, z))
+---@param skip_unstocking boolean | nil
+---@param skip_refueling boolean | nil
+function TurtleController:MoveToPosition(x, y, z, dir, skip_unstocking, skip_refueling)
+    -- print(("Turtle is moving to (%d, %d, %d)..."):format(x, y, z))
 
-    self:DisableMining()
+    -- Store the current position on the stack, so we can return using TurtleController:MoveBack()
     self.last_positions:Push(Position:Copy(self.position))
 
-    if self.position.z > 1 then
-        -- Move south once (if we're at the top) to not be blocked by the slice that is currently mined.
-        -- This assumes that we mine the full width back to front.
-        self:TurnToDirection(Direction.SOUTH)
-        self:MoveRelative(1)
-    elseif self.position.z == 0 then
-        -- Move north once (if we're at the bottom) to not be blocked by the chests
-        self:TurnToDirection(Direction.NORTH)
-        self:MoveRelative(1)
-    end
-
-    -- EAST/WEST axis (do first to not interfere with chests or unmined walls)
+    -- EAST/WEST axis (do first to not interfere with chests)
     if self.position.x > x then
         self:TurnToDirection(Direction.WEST)
     elseif self.position.x < x then
         self:TurnToDirection(Direction.EAST)
     end
-    self:MoveRelative(math.abs(self.position.x - x))
+    self:MoveForward(math.abs(x - self.position.x), skip_unstocking, skip_refueling)
 
     -- NORTH/SOUTH axis
     if self.position.z > z then
@@ -163,28 +208,47 @@ function TurtleController:MoveToPosition(x, y, z, dir)
     elseif self.position.z < z then
         self:TurnToDirection(Direction.NORTH)
     end
-    self:MoveRelative(math.abs(self.position.z - z))
+    self:MoveForward(math.abs(z - self.position.z), skip_unstocking, skip_refueling)
+
+    -- UP/DOWN axis
+    self:MoveVertical(y - self.position.y, skip_unstocking, skip_refueling)
 
     -- Direction
     self:TurnToDirection(dir or Direction.NORTH)
 
     -- Sanity check
     if not(self.position.x == x and self.position.y == y and self.position.z == z and self.position.dir == dir) then
-        printError("TurtleController:MoveToPosition failed to move to target position!")
+        error("TurtleController:MoveToPosition failed to move to target position!")
     end
 end
 
 
-function TurtleController:MoveBack()
-    local target_position = self.last_positions:Pop()
+---Move by a vector. Stores the current position on the stack so we can return using TurtleController:MoveBack()
+---@param x number The EAST/WEST axis (grows from WEST to EAST)
+---@param y number The UP/DOWN axis (grows from DOWN to UP)
+---@param z number The NORTH/SOUTH axis (grows from SOUTH to NORTH)
+---@param dir Direction
+---@param skip_unstocking boolean | nil
+---@param skip_refueling boolean | nil
+function TurtleController:MoveByVector(x, y, z, dir, skip_unstocking, skip_refueling)
+    self:MoveToPosition(self.position.x + x, self.position.y + y, self.position.z + z, dir, skip_unstocking, skip_refueling)
+end
 
-    if target_position == nil then
-        shell.exit()
+
+---Move to the previously stored position and pop this position from the stack.
+---@param skip_unstocking boolean | nil
+---@param skip_refueling boolean | nil
+function TurtleController:MoveBack(skip_unstocking, skip_refueling)
+    local last_position = self.last_positions:Pop()
+
+    if last_position == nil then
+        error("Failed to obtain last_position to move back!")
     else
-        self:MoveToPosition(target_position.x, target_position.y, target_position.z, target_position.dir)
+        self:MoveToPosition(last_position.x, last_position.y, last_position.z, last_position.dir, skip_unstocking, skip_refueling)
+
+        -- Pop the stack because MoveToPosition pushes our current position
         self.last_positions:Pop()
     end
-
 end
 
 
@@ -193,6 +257,27 @@ end
 -----------------------------------------------------------------------------------------------
 
 
+---@return boolean
+function TurtleController:HasFuel()
+    local level = turtle.getFuelLevel()
+    local distance_home = self.position.x + self.position.y + self.position.z
+
+    return level > distance_home + self.config.refuel_safety_margin
+end
+
+
+---@return boolean
+function TurtleController:HasInventorySpace()
+    for slot = 1,16 do
+        if turtle.getItemDetail(slot) == nil then
+            return true
+        end
+    end
+
+    return false
+end
+
+
 ---@param slot number
 ---@param count number
 ---@return boolean
@@ -241,14 +326,17 @@ function TurtleController:UnstockIfFull(skip_inventory_check)
     if not skip_inventory_check and self:HasInventorySpace() then
         return
     end
+    if skip_inventory_check then
+        print(("HasInventorySpace() returned %s"):format(self:HasInventorySpace()))
+    end
 
     print("Turtle is unstocking...")
 
-    self:MoveToPosition(0, 0, 0, self.config.storage_direction)
+    self:MoveToPosition(0, 0, 0, self.config.storage_direction, true, true)
     self:DropInventory()
     self:RefuelIfEmpty()
 
-    self:MoveBack()
+    self:MoveBack(true, true)
 end
 
 
@@ -258,11 +346,14 @@ function TurtleController:RefuelIfEmpty(skip_fuel_check)
     if not skip_fuel_check and self:HasFuel() then
         return
     end
+    if skip_fuel_check then
+        print(("HasFuel() returned %s"):format(self:HasFuel()))
+    end
 
     print("Turtle is refueling...")
 
     -- Clear our inventory into the storage chest
-    self:MoveToPosition(0, 0, 0, self.config.storage_direction)
+    self:MoveToPosition(0, 0, 0, self.config.storage_direction, true, true)
     self:DropInventory()
 
     -- Prepare refueling
@@ -274,7 +365,7 @@ function TurtleController:RefuelIfEmpty(skip_fuel_check)
     local target_fuel_level = self.config.refuel_amount
     local last_position = self.last_positions:Peek()
     if last_position == nil then
-        shell.exit()
+        error("Failed to obtain last_position while refueling!")
     else
         target_fuel_level = target_fuel_level + last_position.x + last_position.y + last_position.z
     end
@@ -283,14 +374,13 @@ function TurtleController:RefuelIfEmpty(skip_fuel_check)
     local before_level = turtle.getFuelLevel()
     repeat
         if not self:SuckItem(1, 1) then
-            printError("Failed to suck fuel out of fuel chest!")
-            shell.exit()
+            error("Failed to suck fuel out of fuel chest!")
         end
         turtle.refuel()
     until turtle.getFuelLevel() >= target_fuel_level
     local after_level = turtle.getFuelLevel()
 
-    self:MoveBack()
+    self:MoveBack(true, true)
 
     print(("Refuelled %d units, current level is %d (old level was %d)"):format(after_level - before_level, after_level, before_level))
 end
@@ -334,32 +424,34 @@ function TurtleController:Configure()
 end
 
 
-function TurtleController:EnableMining()
-    self.mine = true
+function TurtleController:EnableMiningForward()
+    self.mine_forward = true
 end
 
 
-function TurtleController:DisableMining()
-    self.mine = false
+function TurtleController:DisableMiningForward()
+    self.mine_forward = false
 end
 
 
-function TurtleController:HasFuel()
-    local level = turtle.getFuelLevel()
-    local distance_home = self.position.x + self.position.y + self.position.z
-
-    return level > distance_home + self.config.refuel_safety_margin
+function TurtleController:EnableMiningAbove()
+    self.mine_above = true
 end
 
 
-function TurtleController:HasInventorySpace()
-    for slot = 1,16 do
-        if turtle.getItemDetail(slot) == nil then
-            return true
-        end
-    end
-
-    return false
+function TurtleController:DisableMiningAbove()
+    self.mine_above = false
 end
 
+
+function TurtleController:EnableMiningBelow()
+    self.mine_below = true
+end
+
+
+function TurtleController:DisableMiningBelow()
+    self.mine_below = false
+end
+
+
 return TurtleController
\ No newline at end of file