ComputerCraft FTB Infinity Build Platform Utilities

-- Utilities for my FTB Infinity
-- This is filled with utilities that I wrote for my robot turtles to
-- do things and stuff
-- (BuildPlatformUtilities - bpu)

-- This API is defined: http://pastebin.com/2Wk9RVTK
os.loadAPI("MatthewC/tst") -- Replace with new name of the turtle self tracking

local transitY = 110 -- The height the turtle will travel at going between its work, and the waystation

local refuelX = 999 -- X-location of refueling
local refuelY = 101 -- Y-location of refueling
local refuelZ = 944 -- Z-location of refueling
local refuelOrientation = "E" -- Orientation of refueling

local restockX = 1006
local restockY = 101
local restockZ = 944
local restockOrientation = "E" -- Orientation of restocking on building supplies

-- The vector to start probing for the next row to build
local findRowStartX = 1003
local findRowStartY = 61
local findRowStartZ = 914
local findRowStartOrientation = "N"
local buildStartOrientation = "E" -- Stores the location the robot starts building in.
                                  --The robot will circle around to the right always. (clockwise)

local platformHeight = 8

local modem = peripheral.wrap("left") -- Put in the location of the modem here. you BETTER have one.
local modemPort = 25565
local errorPort = 9001 -- The port for reporting errors.
local turtleName = "Turtle1" -- Put the name of the turtle here.


-- Stores the status of the robot's current state.
-- 1 : Refueling
-- 2 : Restocking
-- 3 : Searching for next location to place blocks (once a location is found, cache the start location, and
-- 4 : Building platform
-- 5 : Return to build location
status = 3

-- Caches where we started building
local startBuildX = nil
local startBuildY = nil
local startBuildZ = nil

-- Caches where we are currently building
local currentBuildX = nil
local currentBuildY = nil
local currentBuildZ = nil
local currentBuildOrientation = nil

fileSaveLoc = "MatthewC/appdata/turtleStatus" -- The directory to save / load the cached values out of

-- Build a string that indicates the fuel reserves of the Turtle
function buildStringOfFuel()
  return "Fuel: "..(100 * (turtle.getFuelLevel() / turtle.getFuelLimit())).."%"
end

function getFuelPercent() -- Calculate the percentage level of the turte
  return 100 * (turtle.getFuelLevel() / turtle.getFuelLimit())
end

function updateStatus()
  --write("Sent a message to modem at "..modemPort.."\r\n")
  modem.transmit(modemPort, 100, tst.getLocationAsString().." "..buildStringOfFuel())
end

function sendMessage(message, port)
  modem.transmit(modemPort, port, turtleName.." "..message)
end

function sendError(message)
  modem.transmit(modemPort, errorPort, turtleName.." ***ERROR*** "..message)
end

function loadCurrentState()
  write("Loading current state\r\n")
  h = fs.open(fileSaveLoc, "r")

  if not h then
    write("It appears that we have no file at: "..fileSaveLoc.."\r\n")
    status = 3
    startBuildX = nil
    startBuildY = nil
    startBuildZ = nil
    currentBuildX = nil
    currentBuildY = nil
    currentBuildZ = nil
    currentBuildOrientation = nil
    return
  end

  local testStatus = h.readLine()
  if testStatus and testStatus ~= "" then
    status = tonumber(testStatus)
    startBuildX = tonumber(h.readLine())
    startBuildY = tonumber(h.readLine())
    startBuildZ = tonumber(h.readLine())
    currentBuildX = tonumber(h.readLine())
    currentBuildY = tonumber(h.readLine())
    currentBuildZ = tonumber(h.readLine())
    currentBuildOrientation = h.readLine()

    if not currentBuildOrientation then
      write("Failed to properly load the orientation")
      sendMessage("Failed to properly load the orientation", 101)
    else
      write("Loaded the file... Orientation = "..currentBuildOrientation)
    end
  end

  h.close()
end

function purgeInventory() -- Function to purge the entire inventory of a turtle
  for i = 1, 16, 1 do
    turtle.select(i)
    turtle.drop()
  end

  turtle.select(1)
end

function safeWriteNilVal(valToWrite) -- Safely builds a string for values that may be nil
  if not valToWrite then
    return ""
  end

  return valToWrite
end

function saveCurrentState()
  -- Build a string with the data to save.
  saveData =
    safeWriteNilVal(status).."\r\n"..
    safeWriteNilVal(startBuildX).."\r\n"..
    safeWriteNilVal(startBuildY).."\r\n"..
    safeWriteNilVal(startBuildZ).."\r\n"..
    safeWriteNilVal(currentBuildX).."\r\n"..
    safeWriteNilVal(currentBuildY).."\r\n"..
    safeWriteNilVal(currentBuildZ).."\r\n"..
    safeWriteNilVal(currentBuildOrientation).."\r\n"

  h = fs.open(fileSaveLoc, "w")
  h.write(saveData)
  h.flush() -- I think this is unnecessary, but meh...
  h.close()

  if not currentBuildOrientation then
    sendError("Current Orientation is nil. (Machuga14, remove this message in the future)")
  end

end

-- Helper function; calculates the distance between two vectors.
-- Distance is technically calculated with a "Manhatten Heuristic"
function getDistanceBetweenVectors(x1, y1, z1, x2, y2, z2)
  local xDist = math.abs(x2-x1)
  local yDist = math.abs(y2-y2)
  local zDist = math.abs(z2-z1)

  return xDist + yDist + zDist
end

function goDirectToVector(x, y, z)
  --sendMessage("Traveling to (x,y,z) of ("..x..", "..y..", "..z..")", 101)
  local xPos, yPos, zPos, orientation = tst.getLocation()

  -- This is really lazy, but I don't care... I just need to
  -- Mostly work under stringent conditions.

  -- Keeps track if we ever fail to rotate.
  local hasFailed = false

  -- Travel to the corresponding Y-Location
  if y > yPos then
    for i = 0, y - yPos - 1, 1 do
      hasFailed = not tst.up()

      if hasFailed then
        sendMessage("Failed to travel to Y-Location", 101)
        return false
      end
    end
  elseif y < yPos - 1 then
    for i = 0, yPos - y - 1, 1 do
      hasFailed = not tst.down()

      if hasFailed then
        sendMessage("Failed to travel to Y-Location", 101)
        return false
      end
    end
  end

  updateStatus()

  -- Travel to the corresponding X-Location
  -- Note: X-Location is your East/Westness
  -- Orient to the correct direction
  if x > xPos then
    tst.turnToOrientation("E") -- We need to travel East
  elseif x < xPos then
    tst.turnToOrientation("W") -- We need to travel West
  end

  -- Move to the correct X-pos
  for i = 0, math.abs(x-xPos) - 1, 1 do
    hasFailed = not tst.forward()

    if hasFailed then
      sendMessage("Failed to travel to Y-Location", 101)
      return false
    end
  end

  updateStatus()

  -- Travel to the corresponding Z-Location
  -- Note: z-Location is your North/Southness
  -- Orient to the correct direction
  if z < zPos then
    tst.turnToOrientation("N") -- We need to travel North
  elseif z > zPos then
    tst.turnToOrientation("S") -- We need to travel South
  end

  -- Move to the correct Z-pos
  for i = 0, math.abs(z-zPos) - 1, 1 do
    hasFailed = not tst.forward()

    if hasFailed then
      sendMessage("Failed to travel to Y-Location", 101)
      return false
    end
  end

  updateStatus()
  return true
end

-- Designed to safely go to a specified vector, by traveling to the y location, then the x location, then the z location.
function travelToVector(x, y, z, direction)
  local hasFailed = false
  local xPos, yPos, zPos, orientation = tst.getLocation()

  -- Check to see if we are already at the specified location. If so, let's just turn to the appropriate orientation.
  if x == xPos and y == yPos and z == zPos then
    if direction then
      return tst.turnToOrientation(direction)
    end

    return true
  end

  -- Travel to y location
  hasFailed = not goDirectToVector(xPos, transitY, zPos)
  if hasFailed then
    sendMessage("Safe Travel To Vector Failed to travel to Y-Location", 101)
    return false
  end

  -- Travel to x location
  hasFailed = not goDirectToVector(x, transitY, zPos)
  if hasFailed then
    sendMessage("Safe Travel to Vector failed to travel to X-Location", 101)
    return false
  end

  -- Travel to z location
  hasFailed = not goDirectToVector(x, transitY, z)
  if hasFailed then
    return false
  end

  -- Travel to y location
  hasFailed = not goDirectToVector(x, y, z)
  if hasFailed then
    sendMessage("Safe Travel to Vector failed to travel to Z-Location", 101)
    return false
  end

  if direction then
    hasFailed = not tst.turnToOrientation(direction)
  end
  if hasFailed then
    sendMessage("Safe Travel to Vector failed to Orient to "..direction, 101)
    return false
  end

  updateStatus()
  return true
end

function headToRefuel() -- Function for returning to refuel
  if getFuelPercent() > 98 then
    return true
  end


  local xPos, yPos, zPos, orientation = tst.getLocation()

  local hasFailed = false

  sendMessage("Beginning to refuel", 101)
  turtle.select(1) -- Drop everything in slot 1, since that is what we'll use to refuel.
  turtle.drop()
  hasFailed = not travelToVector(refuelX, refuelY, refuelZ, refuelOrientation)
  if (hasFailed) then
    sendError("Failed to travel to refuel station")
    updateStatus()
    return false
  end

  -- Code here to pick up blocks of fuel, and refuel self
  sendMessage("Arrived at refuel depo. Beginning to refuel", 101)
  turtle.select(1)

  while getFuelPercent() < 98 do
    while not turtle.suck() do -- keep sucking until suck is successful
    end

    if not turtle.refuel() then
      tst.up()
      tst.turnRight()
      purgeInventory()
      tst.turnLeft()
      tst.down()
    end

    updateStatus()
  end

  sendMessage("Refueled. Returning to original location", 101)

  hasFailed = not travelToVector(xPos, yPos, zPos, orientation)

  if (hasFailed) then
    sendError("Failed to travel from refuel entry to original location")
    updateStatus()
    return false
  end

  sendMessage("Successfully Refueled and Returned.", 101)
  return true
end

function checkIfEmpty() -- Returns true if nothing is in the inventory, and false if something is in the inventory
  for i = 1, 16, 1 do
    turtle.select(i)
    if turtle.getItemCount() > 0 then
      return false
    end
  end

  return true
end

function headToPlatformBuildingChest() -- Function for restocking on supplies for building the platform
  if not checkIfEmpty() then
    return true
  end

  local xPos, yPos, zPos, orientation = tst.getLocation()

  local hasFailed = false

  sendMessage("Beginning to restock on building supplies. Direction ="..orientation, 101)
  local hasFailed = not travelToVector(restockX, restockY, restockZ, restockOrientation)
  if (hasFailed) then
    sendError("Failed to travel to restock station")
    updateStatus()
    return false
  end

  -- Code here to pick up blocks from the building supply chest
  for i = 1, 16, 1 do
    turtle.select(i)

    while not turtle.suck(64) do -- Keep sucking until a stack is successfully sucked.
    end
  end

  sendMessage("Restocked. returning to station.", 101)

  hasFailed = not travelToVector(xPos, yPos, zPos, orientation)

  if (hasFailed) then
    sendError("Failed to travel from refuel entry to original location")
    updateStatus()
    return false
  end

  sendMessage("Successfully Restocked and Returned. Orientation="..orientation, 101)
  return true
end

function travelToYLocation(yLocation)
  local xPos, yPos, zPos, orientation = tst.getLocation()

  -- Try to move up indefinitely until we hit the appropriate height for the platform.
  while math.abs(yPos - yLocation) ~= 0 do
    if yPos < yLocation then
      local success = tst.up()
      updateStatus()

      if not success then
        return false
      end
    else
      local success = tst.down()
      updateStatus()

      if not success then
        return false
      end
    end
    xPos, yPos, zPos, orientation = tst.getLocation()
  end

  return true -- We have traveled to the desired y-location
end

function stubbornTravelToYLocation(yLocation) -- Infinitely try to travel to a y location.
  while not travelToYLocation(yLocation) do end
end

function travelToYLocationCacheCurrentLocation(yLocation)
  currentBuildX, currentBuildY, currentBuildZ, currentBuildOrientation = tst.getLocation()

  -- Try to move up indefinitely until we hit the appropriate height for the platform.
  while math.abs(currentBuildY - yLocation) ~= 0 do
    if currentBuildY < yLocation then
      local success = tst.up()
      updateStatus()

      if not success then
        return false
      end
    else
      local success = tst.down()
      updateStatus()

      if not success then
        return false
      end
    end

    currentBuildX, currentBuildY, currentBuildZ, currentBuildOrientation = tst.getLocation()
    saveCurrentState()
  end

  return true -- We have traveled to the desired y-location
end

function findNextBuildRow() -- Find the next row for building
  -- First, travel to the start location
  if not travelToVector(findRowStartX, findRowStartY, findRowStartZ, findRowStartOrientation) then
    sendError("Failed to travel to the start location for finding the next row to build")
    updateStatus()
    return false
  end

  -- At this point, we have found our first build location.
  local xPos, yPos, zPos, orientation = tst.getLocation()
  stubbornTravelToYLocation(findRowStartY + platformHeight)

  -- Start "Probing" For the next actual build location.
  local foundX, foundY, foundZ = nil

  while not foundX do
    -- 1. Keep trying to move forward indefinately
    while not tst.forward() do end

    -- Keep trying to move down until we can't, or until we find the "bottom".
    -- Once we have found the "bottom", we have found our build location.
    if not travelToYLocation(findRowStartY) then
      stubbornTravelToYLocation(findRowStartY + platformHeight)
    else
      if not turtle.detectDown() then -- Return we have found a valid location if and only if there is no block below us.
        foundX, foundY, foundZ = tst.getLocation()
      else
        stubbornTravelToYLocation(findRowStartY + platformHeight)
      end
    end
  end

  tst.turnToOrientation(buildStartOrientation)
  return foundX, foundY, foundZ
end

function equipNextStack() -- returns true if a stack was equipped. returns false if it was empty.
  write("Equipping next stack\r\n")
  for i = 1, 16, 1 do
    if turtle.getItemCount(i) > 0 then
      turtle.select(i)
      write("Found a stack at inventory["..i.."]")
      return true
    end
  end

  write("Detected that the inventory is empty...:(\r\n")
  return false -- This means we will need to restock
end

function buildColumn(top) --Builds a column, moving up until the bot gets to top in the ylocation
  currentBuildX, currentBuildY, currentBuildZ, currentBuildOrientation = tst.getLocation()

  write("building a column\r\n")

  if not equipNextStack() then
    return false
  end

  -- Build the column.
  while currentBuildY < top do
    while not turtle.detectDown() and not turtle.placeDown() do -- Keep trying to place a block below the turtle
      if not equipNextStack() then
        return false
      end
    end
    local success = tst.up()
    if not success then
      updateStatus()
      sendError("Seemingly stuck while trying to build column at current location. Waiting 5 seconds")
      os.sleep(5)
    end

    currentBuildX, currentBuildY, currentBuildZ, currentBuildOrientation = tst.getLocation()
    saveCurrentState()
  end

  return true -- We have built a column to the desired y-location
end

function buildRow(finishXLoc, finishYLoc, finishZLoc) -- build the current row the robot is located at.
  currentBuildX, currentBuildY, currentBuildZ, currentBuildOrientation = tst.getLocation()
  saveCurrentState()
  travelToYLocationCacheCurrentLocation(findRowStartY) -- Make sure we are at the bottom of the current row

  local failToTurnRightCount = 0 -- Keep track of how many times we have tried to turn right, and failed.
          --If the count exceeds 2 in a row, we have an error, and should return 0.

  -- While-Loop to build the current column, move forward, drop, and repeat.
  while true do
    if failToTurnRightCount == 1 then -- We have failed to turn right, so lets try reorienting ourselves left.
      tst.back()
      tst.turnLeft()
      tst.forward()
      tst.down()
      if turtle.detectDown() then -- Return we are finished if we cannot turn left.
        return 0
      end

      travelToYLocationCacheCurrentLocation(findRowStartY) -- Make sure we are at the bottom of the current row (since we just moved here...)
    end

    if not buildColumn(findRowStartY + platformHeight) then
      return 2 -- We are out of building supplies. Time to restock!
    end

    if not tst.forward() then
      return 0 -- We are done; we ran into an obstacle. We must have finished.
    end

    currentBuildX, currentBuildY, currentBuildZ, currentBuildOrientation = tst.getLocation()
    saveCurrentState()

    -- We have successfully moved forward. If we have successfully moved forward, and our X and Z locations match the finish, we're done.
    if currentBuildX == finishXLoc and currentBuildZ == finishZLoc then
      return 0
    end

    -- At this point, we just need to try to travel back down to the findRowStartY, and build another platform
    travelToYLocationCacheCurrentLocation(findRowStartY)
    currentBuildX, currentBuildY, currentBuildZ, currentBuildOrientation = tst.getLocation()
    saveCurrentState()

    -- Only try to turn right if we are at the bottom of the column. If we couldn't get there, keep moving forwards.
    if findRowStartY == currentBuildY then
      failToTurnRightCount = 0 -- Indicate that we have successfully turned right, so reset the counter to 0.
      -- Check to the right of the turtle. If it's empty, we need to stay right
      tst.turnRight()
      currentBuildX, currentBuildY, currentBuildZ, currentBuildOrientation = tst.getLocation()
      saveCurrentState()
      tst.down()
      currentBuildX, currentBuildY, currentBuildZ, currentBuildOrientation = tst.getLocation()
      saveCurrentState()
      if turtle.detect() then
        tst.turnLeft()
        currentBuildX, currentBuildY, currentBuildZ, currentBuildOrientation = tst.getLocation()
        saveCurrentState()
      end
      while not tst.up() do
        currentBuildX, currentBuildY, currentBuildZ, currentBuildOrientation = tst.getLocation()
        saveCurrentState()
        os.sleep(3)
      end -- Keep forcing itself to go up
    else
      failToTurnRightCount = failToTurnRightCount + 1
    end -- End If findRowStartY == currentBuildY
  end -- End while loop
end -- End Function








---------------------------------------------------
--
-- Code to run BuildPlatform here
---------------------------------------------------
function buildPlatform()
loadCurrentState()
updateStatus()
sendMessage("Booted up; Starting to Build Platform", 101)

local xpos, ypos, zpos, orientation = tst.getLocation()
if status == 4 and (xpos ~= currentBuildX or zpos ~= currentBuildZ) then
  write("Anomaly detected: turtlebot not where expected. Traveling to expected location.\r\n")
  write("Current "..tst.getLocationAsString().."\r\n")
  write("Expected Location: x="..safeWriteNilVal(currentBuildX).." y="..safeWriteNilVal(currentBuildY).." z="..safeWriteNilVal(currentBuildZ).." Orientation="..safeWriteNilVal(currentBuildOrientation).."\r\n")
  sendError("Anomaly detected: turtlebot not where expected. Traveling to expected location.")
  sendError("current "..tst.getLocationAsString())
  sendError("Expected Location: x="..currentBuildX.." y="..currentBuildY.." z="..currentBuildZ.." Orientation="..currentBuildOrientation)
  while not travelToVector(currentBuildX, currentBuildY, currentBuildZ, currentBuildOrientation) do -- Keep trying to travel to the appropriate vector.
    updateStatus()
    os.sleep(1)
  end
end

while true do
  if status == 1 then
    while not headToRefuel() do end -- Continuously try to refuel
    status = 5
    saveCurrentState()
  elseif status == 2 then
    write("Need to head to platform")
    while not headToPlatformBuildingChest() do end -- Continuously try to restock
    write("Done heading to platform. Setting status to 5.")
    status = 5
    saveCurrentState()
  elseif status == 3 then
    sendMessage("Searching for next build row", 101)
    startBuildX = nil
    startBuildY = nil
    startBuildZ = nil
    currentBuildX = nil
    currentBuildY = nil
    currentBuildZ = nil
    currentBuildOrientation = nil

    if getFuelPercent() < 30 then -- If we are at less than 30% fuel, we should refuel.
      status = 1
    else
      while not startBuildX do
        startBuildX, startBuildY, startBuildZ = findNextBuildRow()
        if not startBuildX then
          write("Failed to find next row. trying again.")
          write("Looked for row at: (x,y,z) ("..startBuildX..", "..startBuildY..", "..startBuildZ..")")
          sendMessage("Failed to find next row. trying again.",101)
          sendMessage("Looked for row at: (x,y,z) ("..startBuildX..", "..startBuildY..", "..startBuildZ..")",101)
          os.sleep(5)
        end
      end

      write("Found a location to build.")
      sendMessage("Found a location to build.", 101)
      status = 4
    end
    saveCurrentState()

  elseif status == 4 then

    buildAnswer = buildRow(startBuildX, startBuildY, startBuildZ)

    currentBuildX, currentBuildY, currentBuildZ, currentBuildOrientation = tst.getLocation()

    if buildAnswer == 0 then
      status = 3 -- We need to restart and work on the next row.

      -- mark that we no longer have a current build location.
      currentBuildX = nil
      currentBuildY = nil
      currentBuildZ = nil
      currentBuildOrientation = nil
    elseif buildAnswer == 1 then
      status = 1
    elseif buildAnswer == 2 then
      status = 2
    elseif buildAnswer then
      sendError("encountered an unexpected return from buildRow(): "..buildAnswer, 101)
    else
      sendError("Encountered a nil return from buildRow()")
    end

  saveCurrentState()
  elseif status == 5 then
    write("Status is 5\r\n")
    if not currentBuildX then
      status = 3 -- If we do not currently have a valid build location, lets start finding our next build location.
      write("Detected we probably should start a new row\r\n")
    else
      write("Detected we need to travel to our current build location. Doing so\r\n")
      sendMessage("Returning to Stage 4, with a direction of"..currentBuildOrientation, 101)
      while not travelToVector(currentBuildX,currentBuildY,currentBuildZ,currentBuildOrientation) do end
      write("Traveled to curret build location. Continuing operation of building base.")
      status = 4
    end
  saveCurrentState()
  else
    status = 1
    saveCurrentState()
  end -- End if
end -- While true
end -- Function BuildPlatform