Pathfinding for turtles

-- Variables
xPos, yPos, zPos = nil
XLocation, YLocation, ZLocation = nil
hx = homeX
hy = homeY
hz = homeZ
face = 1
side = nil
cal = false
-- end Variables

--locations
function homeXLoc()
local Xhome = fs.open("homeX", "w")
Xhome.writeLine(XLocation)
Xhome.close()
end

function homeYLoc()
local Yhome = fs.open("homeY", "w")
Yhome.writeLine(YLocation)
Yhome.close()
end

function homeZLoc()
local Zhome = fs.open("homeZ", "w")
Zhome.writeLine(ZLocation)
Zhome.close()
end

function readXLoc()
XLoc = fs.open("homeX", "r")
homeX = tonumber (XLoc.readLine())
XLoc.close()
end

function readYLoc()
YLoc = fs.open("homeY", "r")
homeY = tonumber(YLoc.readLine())
YLoc.close()
end

function readZLoc()
ZLoc = fs.open("homeZ", "r")
homeZ = tonumber(ZLoc.readLine())
ZLoc.close()
end
--end Locations

--functions
function setLocation() -- get gps using other computers
 xPos, yPos, zPos = gps.locate()
 cal = true
end

function setHome()
    setLocation()
    XLocation = xPos
    YLocation = yPos
    ZLocation = zPos
    homeXLoc()
    homeYLoc()
    homeZLoc()
end

function getSide()
    if face == 1 then
        side = "West"
    elseif face == 2 then
        side = "South"
    elseif face == 3 then
        side = "East"
    elseif face == 0 then
        side = "North"
    end
end

function getLocation() -- return the location
 if xPos ~= nil then
  print(xPos, yPos, zPos)
 else
  return nil
 end
end

function turnLeft() -- turn left
turtle.turnLeft()
 if face == 0 then
  face = 1
 elseif face == 1 then
  face = 2
 elseif face == 2 then
  face = 3
 elseif face == 3 then
  face = 0
 end
end

function turnRight() -- turn right
turtle.turnRight()
 if face == 0 then
  face = 3
 elseif face == 1 then
  face = 0
 elseif face == 2 then
  face = 1
 elseif face == 3 then
  face = 2
 end
end

function forward() -- go forward
 turtle.forward()
 if cal == true then
  if face == 0 then
   zPos = zPos - 1
  elseif face == 1 then
   xPos = xPos - 1
  elseif face == 2 then
   zPos = zPos + 1
  elseif face == 3 then
   xPos = xPos + 1
  end
 else
  print("Not Calibrated.")
 end
end

function back() -- go back
 turtle.back()
 if cal == true then
  if face == 0 then
   zPos = zPos + 1
  elseif face == 1 then
   xPos = xPos + 1
  elseif face == 2 then
   zPos = zPos - 1
  elseif face == 2 then
   xPos = xPos - 1
  end
 else
  print("Not Calibrated.")
 end
end

function up() -- go up
 turtle.up()
 if cal == true then
  yPos = yPos + 1
 else
  print("Not Calibrated.")
 end
end

function down() -- go down
turtle.down()
    if cal == true then
        yPos = yPos - 1
    else
        print("Not Calibrated.")
    end
end

function sort()
    for i = 1,16 do
        turtle.select(i)
        turtle.suck()
    end
    for i = 1,16 do
        turtle.select(i)
        if turtle.getItemCount() == 0 then
            break
        else
            table.insert(items, i, turtle.getItemDetail().name)
            table.insert(amount, i, turtle.getItemCount())
        end
    end

    for k,v in pairs(items) do
        print(v)
    end
end

function goHome()
--setLocation()
--getLocation()
readXLoc()
readYLoc()
readZLoc()
    while zPos ~= homeZ do
        if zPos < homeZ then
            if side ~= "South" then
                repeat
                turnRight()
                getSide()
                until side == "South"
            end
            repeat forward() until zPos == homeZ
            turnRight()
        else
            if side ~= "North" then
                repeat
                turnLeft()
                getSide()
                until side == "North"
            end
            repeat forward() until zPos == homeZ
            turnLeft()
        end
    end
    while xPos ~= homeX do
        if xPos > homeX then
            repeat forward() until xPos == homeX
        else
            repeat back() until xPos == homeX
        end
    end
    while yPos ~= homeY do
        if yPos < homeY then
            repeat up() until yPos == homeY
        else
            repeat down() until yPos == homeY
        end
    end
end

function errorMes()
        print("Don't understand the command.")
        print("Press enter to continue")
            if io.read() == "" then
                term.clear()
                term.setCursorPos(1,1)
                write("Enter the Command: ")
            end
end

function help()
print("The following commands can be used: ")
print("- sethome or home")
print("- sort")
print("- go home")
print("- help OR ?")
print("- exit or quit")
print("")
end
-- end functions
setLocation()
term.clear()
term.setCursorPos(1,1)
write("Enter the Command: ")
while true do
local input = read()
    if input:lower() == "set home" or input:lower() == "sethome" or input:lower() == "home" then
        setHome()
        print("Home is Set")
        print("Press enter to continue")
            if io.read() == "" then
                term.clear()
                term.setCursorPos(1,1)
                write("Enter the Command: ")
            end
    elseif input:lower() == "help" or input:lower() == "?" then
        help()
        print("Press enter to continue")
            if io.read() == "" then
                term.clear()
                term.setCursorPos(1,1)
                write("Enter the Command: ")
            end
    elseif input:lower() == "exit" or input:lower() == "quit" then
        os.reboot()
    elseif input:lower() == "go home" then
        while xPos ~= homeX do
            repeat
            term.clear()
            term.setCursorPos(1,1)
            print("Turtle Location: ")
            print(xPos..", "..yPos..", "..zPos)
            term.setCursorPos(1,3)
            print("Home Location: ")
            print(hx..", "..hy..", "..hz)
            until xPos == homeX and yPos == homeY and zPos == homeZ
        end
        goHome()
        print("Press enter to continue")
        if io.read() == "" then
            term.clear()
            term.setCursorPos(1,1)
            write("Enter the Command: ")
        end
    else
        errorMes()
    end
end