joeld98

A* in computercraft - Attempt 2

Aug 18th, 2016
25
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. local function log(msg) -- adapted from: https://github.com/rxi/log.lua
  2.   if not os.loadAPI("rwt") then
  3.     if not shell.run("pastebin", "get", "TNLjCCKq", "rwt") then
  4.       print("Could not load Time API")
  5.     else
  6.       os.loadAPI("rwt")
  7.     end
  8.   end
  9.   --logTime = rwt.getTime("utc",-4,dateTime24_mmddyyyy,10)
  10.   logTime = "TIMEOFF"
  11.   local fp = io.open("seek.log", "a")
  12.   --local str = string.format("[%s]%s\n",logTime, msg)
  13.   --local str = string.format("[%s]%s\n",logTime, msg)
  14.   local str = string.format("%s\n",msg)
  15.       fp:write(str)
  16.       fp:close()
  17. end
  18.  
  19. local function saveTable(table,fileName)
  20.   local hWrite = fs.open(fileName, "w")
  21.   hWrite.write(textutils.serialize(table))
  22.   hWrite.close()
  23. end
  24.  
  25. local function getFacingDirection()
  26.     log("Get Facing Direction Called")
  27.     local function findClearBlock()
  28.     local turnsToClear = 0
  29.     while turtle.inspect() and turnsToClear <=4 do
  30.       turtle.turnRight()
  31.       turnsToClear = turnsToClear + 1
  32.     end
  33.     if turnsToClear >= 4 then
  34.       return nil
  35.     else
  36.     end
  37.     if turnsToClear ~=4 and turnsToClear ~=0 then
  38.       for faceHome = 1, turnsToClear do
  39.         turtle.turnLeft()
  40.       end
  41.     end
  42.     return turnsToClear
  43.     end
  44.     local startLoc = vector.new(gps.locate())
  45.     local dirLoc
  46.     local facingDir = 0
  47.     local facingVector
  48.     if turtle.forward() then
  49.     dirLoc = vector.new(gps.locate())
  50.     turtle.back()
  51.     facingVector = dirLoc - startLoc
  52.     facingDir = ((facingVector.x + math.abs(facingVector.x) * 2) + (facingVector.z + math.abs(facingVector.z) * 3))
  53.     else
  54.     local tCount = findClearBlock()
  55.     if tCount == nil then
  56.       return nil
  57.     elseif tCount == 2 then
  58.       turtle.back()
  59.       dirLoc = vector.new(gps.locate())
  60.       turtle.forward()
  61.       facingVector = startLoc - dirLoc
  62.       facingDir = (((facingVector.x + math.abs(facingVector.x) * 2) + (facingVector.z + math.abs(facingVector.z) * 3)))
  63.     elseif tCount == 1 then
  64.       turtle.turnRight()
  65.       turtle.forward()
  66.       turtle.turnLeft()
  67.       dirLoc = vector.new(gps.locate())
  68.       turtle.turnLeft()
  69.       turtle.forward()
  70.       turtle.turnRight()
  71.       facingVector = dirLoc - startLoc
  72.       facingDir = (((facingVector.x + math.abs(facingVector.x) * 2) + (facingVector.z + math.abs(facingVector.z) * 3))) - 1
  73.     elseif
  74.       tCount == 3 then
  75.       turtle.turnLeft()
  76.       turtle.forward()
  77.       turtle.turnRight()
  78.       dirLoc = vector.new(gps.locate())
  79.       turtle.turnRight()
  80.       turtle.forward()
  81.       turtle.turnLeft()
  82.       facingVector = dirLoc - startLoc
  83.       facingDir = ((facingVector.x + math.abs(facingVector.x) * 2) + (facingVector.z + math.abs(facingVector.z) * 3)) + 1
  84.       if facingDir > 4 then
  85.       facingDir = 1
  86.       end
  87.     end
  88.     end
  89.     log(string.format("Facing:%s", tostring(facingDir)))
  90.     return facingDir
  91. end
  92.  
  93. local function getNodeData(currentVector, targetVector , startVector, parentVector)
  94.   log("GetNodeData Called")
  95.   local cNode, cNodeKey = nil
  96.   local fNode, fNodeKey, fInspect, fResult = nil
  97.   local uNode, uNodeKey, uInspect,uResult = nil
  98.   local dNode, dNodeKey, dInspect, dResult = nil
  99.   local rNode, rNodeKey, rInspect, rResult = nil
  100.   local bNode, bNodeKey, bInspect, bResult = nil
  101.   local lNode, lNodeKey, lInspect, lResult = nil
  102.   cNode = {x = 0, y = 0, z = 0}
  103.   fNode = {x = 0, y = 0, z = 0}
  104.   uNode = {x = 0, y = 0, z = 0}
  105.   dNode = {x = 0, y = 0, z = 0}
  106.   rNode = {x = 0, y = 0, z = 0}
  107.   bNode = {x = 0, y = 0, z = 0}
  108.   lNode = {x = 0, y = 0, z = 0}
  109.   facingDirection = getFacingDirection() -- need this to calculate neighbor offsets and move commands
  110.   if facingDirection == 1 then -- when facing west
  111.     if currentVector.x <= 0 then -- and we are in negative X coordinates
  112.       fNode.x = currentVector.x - 1 -- the node in front of us is our current x value - 1
  113.       fNode.z = currentVector.z
  114.       fNode.y = currentVector.y
  115.       bNode.x = currentVector.x + 1 -- the node in behind our current x value is + 1
  116.       bNode.z = currentVector.z
  117.       bNode.y = currentVector.y
  118.     else
  119.       fNode.x = currentVector.x + 1 -- when x is a possitive coordinate front is +1
  120.       fNode.z = currentVector.z
  121.       fNode.y = currentVector.y
  122.       bNode.x = currentVector.x - 1 -- and back is -1
  123.       bNode.z = currentVector.z
  124.       bNode.y = currentVector.y
  125.     end
  126.     if currentVector.z <= 0 then  
  127.       lNode.z = currentVector.z + 1  -- when z is negative left is +1
  128.       lNode.x = currentVector.x
  129.       lNode.y = currentVector.y
  130.       rNode.z = currentVector.z - 1  -- and right is -1
  131.       rNode.x = currentVector.x
  132.       rNode.y = currentVector.y
  133.     else
  134.       lNode.z = currentVector.z - 1  -- and the opposite when positive
  135.       lNode.x = currentVector.x
  136.       lNode.y = currentVector.y
  137.       rNode.z = currentVector.z + 1
  138.       rNode.x = currentVector.x
  139.       rNode.y = currentVector.y
  140.     end
  141.   elseif facingDirection == 2 then -- and so on for each direction. 0 and 3 are mirrors as are 2 and 4
  142.     if currentVector.x <= 0 then
  143.       fNode.x = currentVector.x + 1
  144.       fNode.z = currentVector.z
  145.       fNode.y = currentVector.y
  146.       bNode.x = currentVector.x - 1
  147.       bNode.z = currentVector.z
  148.       bNode.y = currentVector.y
  149.     else
  150.       fNode.x = currentVector.x - 1
  151.       fNode.z = currentVector.z
  152.       fNode.y = currentVector.y
  153.       bNode.x = currentVector.x + 1
  154.       bNode.z = currentVector.z
  155.       bNode.y = currentVector.y
  156.     end
  157.     if currentVector.z <=0 then
  158.       lNode.z = currentVector.z - 1
  159.       lNode.x = currentVector.x
  160.       lNode.y = currentVector.y
  161.       rNode.z = currentVector.z + 1
  162.       rNode.x = currentVector.x
  163.       rNode.y = currentVector.y
  164.     else
  165.       lNode.z = currentVector.z + 1
  166.       lNode.x = currentVector.x
  167.       lNode.y = currentVector.y
  168.       rNode.z = currentVector.z - 1
  169.       rNode.x = currentVector.x
  170.       rNode.y = currentVector.y
  171.     end
  172.   elseif facingDirection == 3 then
  173.     if currentVector.x <= 0 then
  174.       fNode.x = currentVector.x + 1
  175.       fNode.z = currentVector.z
  176.       fNode.y = currentVector.y
  177.       bNode.x = currentVector.x - 1
  178.       bNode.z = currentVector.z
  179.       bNode.y = currentVector.y
  180.     else
  181.       fNode.x = currentVector.x - 1
  182.       fNode.z = currentVector.z
  183.       fNode.y = currentVector.y
  184.       bNode.x = currentVector.x + 1
  185.       bNode.z = currentVector.z
  186.       bNode.y = currentVector.y
  187.     end
  188.     if currentVector.z <=0 then
  189.       lNode.z = currentVector.z - 1
  190.       lNode.x = currentVector.x
  191.       lNode.y = currentVector.y
  192.       rNode.z = currentVector.z + 1
  193.       rNode.x = currentVector.x
  194.       rNode.y = currentVector.y
  195.     else
  196.       lNode.z = currentVector.z + 1
  197.       lNode.x = currentVector.x
  198.       lNode.y = currentVector.y
  199.       rNode.z = currentVector.z - 1
  200.       rNode.x = currentVector.x
  201.       rNode.y = currentVector.y
  202.     end
  203.   elseif facingDirection == 4 then
  204.     if currentVector.x <= 0 then
  205.       fNode.x = currentVector.x - 1
  206.       fNode.z = currentVector.z
  207.       fNode.y = currentVector.y
  208.       bNode.x = currentVector.x + 1
  209.       bNode.z = currentVector.z
  210.       bNode.y = currentVector.y
  211.     else
  212.       fNode.x = currentVector.x + 1
  213.       fNode.z = currentVector.z
  214.       fNode.y = currentVector.y
  215.       bNode.x = currentVector.x - 1
  216.       bNode.z = currentVector.z
  217.       bNode.y = currentVector.y
  218.     end
  219.     if currentVector.z <=0 then
  220.       lNode.z = currentVector.z + 1
  221.       lNode.x = currentVector.x
  222.       lNode.y = currentVector.y
  223.       rNode.z = currentVector.z - 1
  224.       rNode.x = currentVector.x
  225.       rNode.y = currentVector.y
  226.     else
  227.       lNode.z = currentVector.z - 1
  228.       lNode.x = currentVector.x
  229.       lNode.y = currentVector.y
  230.       rNode.z = currentVector.z + 1
  231.       rNode.x = currentVector.x
  232.       rNode.y = currentVector.y
  233.     end
  234.   end
  235.   uNode.x = currentVector.x
  236.   uNode.z = currentVector.z
  237.   uNode.y = currentVector.y + 1  
  238.   dNode.x = currentVector.x
  239.   dNode.z = currentVector.z
  240.   dNode.y = currentVector.y - 1  
  241.   cNodeKey = string.format("%d,%d,%d", currentVector.x, currentVector.y, currentVector.z)
  242.   fNodeKey = string.format("%d,%d,%d", fNode.x, fNode.y, fNode.z)
  243.   uNodeKey = string.format("%d,%d,%d", uNode.x, uNode.y, uNode.z)
  244.   dNodeKey = string.format("%d,%d,%d", dNode.x, dNode.y, dNode.z)
  245.   rNodeKey = string.format("%d,%d,%d", rNode.x, rNode.y, rNode.z)
  246.   bNodeKey = string.format("%d,%d,%d", bNode.x, bNode.y, bNode.z)
  247.   lNodeKey = string.format("%d,%d,%d", lNode.x, lNode.y, lNode.z)
  248.   if not parentVector then
  249.     pNodeKey = string.format("%d,%d,%d", currentVector.x, currentVector.y, currentVector.z)
  250.   else
  251.     pNodeKey = string.format("%d,%d,%d", parentVector.x, parentVector.y, parentVector.z)
  252.   end
  253.   if not nodeList[pNodeKey] then -- this should only happen on the first pass when seekDestination is called. Our last pass currentVector is passed as our parent thereafter.
  254.     log(string.format("Adding pNode:%s to nodeList",pNodeKey))
  255.     nodeList[pNodeKey] = {}
  256.     nodeList[pNodeKey].x = currentVector.x
  257.     nodeList[pNodeKey].y = currentVector.y
  258.     nodeList[pNodeKey].z = currentVector.z
  259.     nodeList[pNodeKey].g = 10
  260.     nodeList[pNodeKey].startx = startVector.x
  261.     nodeList[pNodeKey].starty = startVector.y
  262.     nodeList[pNodeKey].startz = startVector.z
  263.     nodeList[pNodeKey].endx = targetVector.x
  264.     nodeList[pNodeKey].endy = targetVector.y
  265.     nodeList[pNodeKey].endz = targetVector.z
  266.     nodeList[pNodeKey].h = math.abs(nodeList[pNodeKey].x - nodeList[pNodeKey].endx) +  math.abs(nodeList[pNodeKey].y-nodeList[pNodeKey].endy) + math.abs(nodeList[pNodeKey].z-nodeList[pNodeKey].endz)
  267.     nodeList[pNodeKey].f = nodeList[pNodeKey].g + nodeList[pNodeKey].h
  268.     nodeList[pNodeKey].state = "closed"
  269.   elseif nodeList[pNodeKey].state == "open" then
  270.     log("OPEN pNode exists in nodeList. Marking as closed")  
  271.     nodeList[pNodeKey].state = "closed"
  272.   end
  273.   if not nodeList[cNodeKey] then -- this should only happen on the first pass when seekDestination is called. Parent and Current are equal on this pass. One of our last pass nodes should become our current node thereafter.
  274.     log(string.format("Adding cNode:%s to nodeList",cNodeKey))
  275.     nodeList[cNodeKey] = {}
  276.     nodeList[cNodeKey].x = currentVector.x
  277.     nodeList[cNodeKey].y = currentVector.y
  278.     nodeList[cNodeKey].z = currentVector.z
  279.     nodeList[cNodeKey].g = 10
  280.     nodeList[cNodeKey].startx = startVector.x
  281.     nodeList[cNodeKey].starty = startVector.y
  282.     nodeList[cNodeKey].startz = startVector.z
  283.     nodeList[cNodeKey].endx = targetVector.x
  284.     nodeList[cNodeKey].endy = targetVector.y
  285.     nodeList[cNodeKey].endz = targetVector.z
  286.     nodeList[cNodeKey].h = math.abs(nodeList[pNodeKey].x - nodeList[cNodeKey].endx) +  math.abs(nodeList[pNodeKey].y-nodeList[cNodeKey].endy) + math.abs(nodeList[pNodeKey].z-nodeList[cNodeKey].endz)
  287.     nodeList[cNodeKey].f = nodeList[cNodeKey].g + nodeList[cNodeKey].h
  288.     nodeList[cNodeKey].state = "closed"
  289.   elseif nodeList[cNodeKey].state == "open" then
  290.     log("cNode exists in nodeList. Marking as closed")  
  291.     nodeList[cNodeKey].state = "closed"
  292.   end
  293.   if not nodeList[fNodeKey] then
  294.     log(string.format("Adding fNode:%s to nodeList",fNodeKey))
  295.     nodeList[fNodeKey] = {}
  296.     nodeList[fNodeKey].x = fNode.x
  297.     nodeList[fNodeKey].y = fNode.y
  298.     nodeList[fNodeKey].z = fNode.z
  299.     nodeList[fNodeKey].startx = startVector.x
  300.     nodeList[fNodeKey].starty = startVector.y
  301.     nodeList[fNodeKey].startz = startVector.z
  302.     nodeList[fNodeKey].endx = targetVector.x
  303.     nodeList[fNodeKey].endy = targetVector.y
  304.     nodeList[fNodeKey].endz = targetVector.z
  305.     nodeList[fNodeKey].g = math.abs((nodeList[cNodeKey].x-nodeList[fNodeKey].x) + (nodeList[cNodeKey].y-nodeList[fNodeKey].y) + (nodeList[cNodeKey].z-nodeList[fNodeKey].z)) + nodeList[pNodeKey].g
  306.     nodeList[fNodeKey].h = math.abs(nodeList[fNodeKey].x - nodeList[fNodeKey].endx) +  math.abs(nodeList[fNodeKey].y-nodeList[fNodeKey].endy) + math.abs(nodeList[fNodeKey].z-nodeList[fNodeKey].endz)
  307.     nodeList[fNodeKey].f = nodeList[fNodeKey].g + nodeList[fNodeKey].h
  308.     nodeList[fNodeKey].state = "open"
  309.   elseif nodeList[fNodeKey].state == "open" then
  310.     log("fNode exists. Updating Parent node.")
  311.     --put logic here to recalc g if needed.
  312.   end
  313.   if not nodeList[uNodeKey] then
  314.     log(string.format("Adding uNode:%s to nodeList",uNodeKey))
  315.     nodeList[uNodeKey] = {}
  316.     nodeList[uNodeKey].x = uNode.x
  317.     nodeList[uNodeKey].y = uNode.y
  318.     nodeList[uNodeKey].z = uNode.z
  319.     nodeList[uNodeKey].startx = startVector.x
  320.     nodeList[uNodeKey].starty = startVector.y
  321.     nodeList[uNodeKey].startz = startVector.z
  322.     nodeList[uNodeKey].endx = targetVector.x
  323.     nodeList[uNodeKey].endy = targetVector.y
  324.     nodeList[uNodeKey].endz = targetVector.z
  325.     nodeList[uNodeKey].g = math.abs((nodeList[pNodeKey].x-nodeList[uNodeKey].x) + (nodeList[pNodeKey].y-nodeList[uNodeKey].y) + (nodeList[pNodeKey].z-nodeList[uNodeKey].z)) + nodeList[pNodeKey].g
  326.     nodeList[uNodeKey].h = math.abs(nodeList[uNodeKey].x - nodeList[uNodeKey].endx) +  math.abs(nodeList[uNodeKey].y-nodeList[uNodeKey].endy) + math.abs(nodeList[uNodeKey].z-nodeList[uNodeKey].endz)
  327.     nodeList[uNodeKey].f = nodeList[uNodeKey].g + nodeList[uNodeKey].h
  328.     nodeList[uNodeKey].state = "open"
  329.   elseif nodeList[uNodeKey].state == "open" then
  330.     log("uNode exists. Updating Parent node.")
  331.     --put logic here to recalc g if needed.
  332.   end
  333.   if not nodeList[dNodeKey] then
  334.     log(string.format("Adding dNode:%s to nodeList",dNodeKey))
  335.     nodeList[dNodeKey] = {}
  336.     nodeList[dNodeKey].x = dNode.x
  337.     nodeList[dNodeKey].y = dNode.y
  338.     nodeList[dNodeKey].z = dNode.z
  339.     nodeList[dNodeKey].startx = startVector.x
  340.     nodeList[dNodeKey].starty = startVector.y
  341.     nodeList[dNodeKey].startz = startVector.z
  342.     nodeList[dNodeKey].endx = targetVector.x
  343.     nodeList[dNodeKey].endy = targetVector.y
  344.     nodeList[dNodeKey].endz = targetVector.z
  345.     nodeList[dNodeKey].g = math.abs((nodeList[pNodeKey].x-nodeList[dNodeKey].x) + (nodeList[pNodeKey].y-nodeList[dNodeKey].y) + (nodeList[pNodeKey].z-nodeList[dNodeKey].z)) + nodeList[pNodeKey].g
  346.     nodeList[dNodeKey].h = math.abs(nodeList[dNodeKey].x - nodeList[dNodeKey].endx) +  math.abs(nodeList[dNodeKey].y-nodeList[dNodeKey].endy) + math.abs(nodeList[dNodeKey].z-nodeList[dNodeKey].endz)
  347.     nodeList[dNodeKey].f = nodeList[dNodeKey].g + nodeList[dNodeKey].h
  348.     nodeList[dNodeKey].state = "open"
  349.   elseif nodeList[dNodeKey].state == "open" then
  350.     log("dNode exists. Updating Parent node.")
  351.     --put logic here to recalc g if needed.
  352.   end
  353.   if not nodeList[rNodeKey] then
  354.     log(string.format("Adding rNode:%s to nodeList",rNodeKey))
  355.     nodeList[rNodeKey] = {}
  356.     nodeList[rNodeKey].x = rNode.x
  357.     nodeList[rNodeKey].y = rNode.y
  358.     nodeList[rNodeKey].z = rNode.z
  359.     nodeList[rNodeKey].startx = startVector.x
  360.     nodeList[rNodeKey].starty = startVector.y
  361.     nodeList[rNodeKey].startz = startVector.z
  362.     nodeList[rNodeKey].endx = targetVector.x
  363.     nodeList[rNodeKey].endy = targetVector.y
  364.     nodeList[rNodeKey].endz = targetVector.z
  365.     nodeList[rNodeKey].g = math.abs((nodeList[pNodeKey].x-nodeList[rNodeKey].x) + (nodeList[pNodeKey].y-nodeList[rNodeKey].y) + (nodeList[pNodeKey].z-nodeList[rNodeKey].z)) + nodeList[pNodeKey].g
  366.     nodeList[rNodeKey].h = math.abs(nodeList[rNodeKey].x - nodeList[rNodeKey].endx) +  math.abs(nodeList[rNodeKey].y-nodeList[rNodeKey].endy) + math.abs(nodeList[rNodeKey].z-nodeList[rNodeKey].endz)
  367.     nodeList[rNodeKey].f = nodeList[rNodeKey].g + nodeList[rNodeKey].h
  368.     nodeList[rNodeKey].state = "open"
  369.   elseif nodeList[rNodeKey].state == "open" then
  370.     log("rNode exists. Updating Parent node.")
  371.     --put logic here to recalc g if needed.
  372.   end
  373.   if not nodeList[bNodeKey] then
  374.     log(string.format("Adding bNode:%s to nodeList",bNodeKey))
  375.     nodeList[bNodeKey] = {}
  376.     nodeList[bNodeKey].x = bNode.x
  377.     nodeList[bNodeKey].y = bNode.y
  378.     nodeList[bNodeKey].z = bNode.z
  379.     nodeList[bNodeKey].startx = startVector.x
  380.     nodeList[bNodeKey].starty = startVector.y
  381.     nodeList[bNodeKey].startz = startVector.z
  382.     nodeList[bNodeKey].endx = targetVector.x
  383.     nodeList[bNodeKey].endy = targetVector.y
  384.     nodeList[bNodeKey].endz = targetVector.z
  385.     nodeList[bNodeKey].g = math.abs((nodeList[pNodeKey].x-nodeList[bNodeKey].x) + (nodeList[pNodeKey].y-nodeList[bNodeKey].y) + (nodeList[pNodeKey].z-nodeList[bNodeKey].z)) + nodeList[pNodeKey].g
  386.     nodeList[bNodeKey].h = math.abs(nodeList[bNodeKey].x - nodeList[bNodeKey].endx) +  math.abs(nodeList[bNodeKey].y-nodeList[bNodeKey].endy) + math.abs(nodeList[bNodeKey].z-nodeList[bNodeKey].endz)
  387.     nodeList[bNodeKey].f = nodeList[bNodeKey].g + nodeList[bNodeKey].h
  388.     nodeList[bNodeKey].state = "open"
  389.   elseif nodeList[bNodeKey].state == "open" then
  390.     log("bNode exists. Updating Parent node.")
  391.     --put logic here to recalc g if needed.
  392.   end
  393.   if not nodeList[lNodeKey] then
  394.     log(string.format("Adding lNode:%s to nodeList",lNodeKey))
  395.     nodeList[lNodeKey] = {}
  396.     nodeList[lNodeKey].x = lNode.x
  397.     nodeList[lNodeKey].y = lNode.y
  398.     nodeList[lNodeKey].z = lNode.z
  399.     nodeList[lNodeKey].startx = startVector.x
  400.     nodeList[lNodeKey].starty = startVector.y
  401.     nodeList[lNodeKey].startz = startVector.z
  402.     nodeList[lNodeKey].endx = targetVector.x
  403.     nodeList[lNodeKey].endy = targetVector.y
  404.     nodeList[lNodeKey].endz = targetVector.z
  405.     nodeList[lNodeKey].g = math.abs((nodeList[pNodeKey].x-nodeList[lNodeKey].x) + (nodeList[pNodeKey].y-nodeList[lNodeKey].y) + (nodeList[pNodeKey].z-nodeList[lNodeKey].z)) + nodeList[pNodeKey].g
  406.     nodeList[lNodeKey].h = math.abs(nodeList[lNodeKey].x - nodeList[lNodeKey].endx) +  math.abs(nodeList[lNodeKey].y-nodeList[lNodeKey].endy) + math.abs(nodeList[lNodeKey].z-nodeList[lNodeKey].endz)
  407.     nodeList[lNodeKey].f = nodeList[lNodeKey].g + nodeList[lNodeKey].h
  408.     nodeList[lNodeKey].state = "open"
  409.   elseif nodeList[lNodeKey].state == "open" then
  410.     log("lNode exists. Updating Parent node.")
  411.     --put logic here to recalc g if needed.
  412.   end
  413.   local fResult,fInspect = turtle.inspect() -- inspect Left, up and down at no cost.
  414.   local uResult,uInspect = turtle.inspectUp() -- using inspect instead of detect to capture block type
  415.   local dResult,dInspect = turtle.inspectDown() -- might need it later
  416.   turtle.turnRight()
  417.   local rResult,rInspect = turtle.inspect()  -- time/move cost but no fuel cost so get them all
  418.   turtle.turnRight()
  419.   local bResult,bInspect = turtle.inspect()
  420.   turtle.turnRight()
  421.   local lResult,lInspect = turtle.inspect()
  422.   turtle.turnRight()
  423.   if fResult == true then -- if any block is found close the node and keep the block type
  424.     nodeList[fNodeKey].state = "closed"
  425.     nodeList[fNodeKey].block_type = fInspect.name
  426.   end
  427.   if uResult == true then
  428.     nodeList[uNodeKey].state = "closed"
  429.     nodeList[uNodeKey].block_type = uInspect.name
  430.   end
  431.   if dResult == true then
  432.     nodeList[dNodeKey].state = "closed"
  433.     nodeList[dNodeKey].block_type = dInspect.name
  434.   end
  435.   if rResult == true then
  436.     nodeList[rNodeKey].state = "closed"
  437.     nodeList[rNodeKey].block_type = rInspect.name
  438.   end
  439.   if bResult == true then
  440.     nodeList[bNodeKey].state = "closed"
  441.     nodeList[bNodeKey].block_type = bInspect.name
  442.   end
  443.   if lResult == true then
  444.     nodeList[lNodeKey].state = "closed"
  445.     nodeList[lNodeKey].block_type = lInspect.name  
  446.   end
  447.   log(string.format("lResult= %s ", tostring(lResult)))
  448.   for k, v in pairs(nodeList) do -- just cheking that table loaded as expected
  449.     --log(string.format("Node:%s End X:%d End Y:%d End Z:%d", tostring(k), nodeList[k].endx, nodeList[k].endy,nodeList[k].endz))
  450.     log(string.format("%s ; %s ; %d ; %s ; %d", tostring(k), tostring(nodeList[k].state),nodeList[k].f,nodeList[k].g, nodeList[k].h))
  451.   end
  452.   --return nodeList
  453.   return nodeList[parentNodeKey], true
  454. end
  455.  
  456. local function getBestNode(currentVector, targetVector , startVector)
  457.   local bestMove = nil
  458.   for vNode, nTable in pairs(nodeList) do
  459.     if nTable.state == "open" then  
  460.       if bestMove == nil then  
  461.         log(string.format("Setting bestMove to:%s", tostring(vNode)))
  462.         bestMove = vNode
  463.       end
  464.       if nTable.f < nodeList[bestMove].f then  
  465.         bestMove = vNode
  466.       elseif nTable.f == nodeList[bestMove].f then  
  467.         if nTable.h < nodeList[bestMove].h then  
  468.           bestMove = vNode
  469.         elseif nTable.h == nodeList[bestMove].h then
  470.           if nodeList[bestMove].y > currentVector.y then
  471.             if nTable.y == currentVector.y then
  472.               bestMove = vNode
  473.             end
  474.           end
  475.         end
  476.       else
  477.       end
  478.     else
  479.     end
  480.   end
  481.   if nodeList[bestMove].state == "open" then -- make sure the best node is open before finishing
  482.     local bmVector = vector.new(nodeList[bestMove].x,nodeList[bestMove].y,nodeList[bestMove].z)
  483.     return bmVector -- returning node as vector. passing as indect to nodeList did not work
  484.   else
  485.     return false -- if bestMove is not open let caller know.
  486.   end
  487. end
  488.  
  489. local function gotoNode(bnOffset, currentVector)  
  490.   facingDirection = getFacingDirection()  -- using to pick move forward or back
  491.   log(string.format("Facing Direction is :%s", tostring(facingDirection)))
  492.   local moves
  493.   if facingDirection == 1 then
  494.     if bnOffset.x > 0 then
  495.       for moves = 1,math.abs(bnOffset.x) do --offsets should always be 1 but will move more if backtracking
  496.         turtle.forward()
  497.       end
  498.     else
  499.       for moves = 1,math.abs(bnOffset.x) do
  500.         turtle.back()
  501.       end
  502.     end
  503.     if bnOffset.z < 0 then
  504.       turtle.turnLeft()
  505.         for moves = 1,math.abs(bnOffset.z) do
  506.           turtle.forward()
  507.         end
  508.       turtle.turnRight()
  509.     else
  510.       turtle.turnRight()
  511.         for moves = 1,math.abs(bnOffset.z) do
  512.           turtle.forward()
  513.         end
  514.       turtle.turnLeft()
  515.     end
  516.     if bnOffset.y < 0 then
  517.       for moves = 1,math.abs(bnOffset.y) do
  518.         turtle.down()
  519.       end
  520.     elseif bnOffset.y > 0 then
  521.       for moves = 1,math.abs(bnOffset.z) do
  522.         turtle.up()
  523.       end
  524.     end
  525.   elseif facingDirection == 2 then
  526.     if bnOffset.z > 0 then
  527.       for moves = 1,math.abs(bnOffset.z) do
  528.         turtle.back()
  529.       end
  530.     elseif bnOffset.z < 0 then
  531.       for moves = 1,math.abs(bnOffset.z) do
  532.         turtle.forward()
  533.       end
  534.     end
  535.     if bnOffset.x < 0 then
  536.       turtle.turnLeft()
  537.       for moves = 1,math.abs(bnOffset.x) do
  538.         turtle.forward()
  539.       end
  540.       turtle.turnRight()
  541.     elseif bnOffset.x > 0 then
  542.       turtle.turnRight()
  543.       for moves = 1,math.abs(bnOffset.x) do
  544.         turtle.forward()
  545.       end
  546.       turtle.turnLeft()
  547.     end
  548.     if bnOffset.y < 0 then
  549.       for moves = 1,math.abs(bnOffset.y) do
  550.         turtle.down()
  551.       end
  552.     elseif bnOffset.y > 0 then
  553.       for moves = 1,math.abs(bnOffset.z) do
  554.         turtle.up()
  555.       end
  556.     end
  557.   elseif facingDirection == 3 then
  558.     if bnOffset.x > 0 then
  559.       for moves = 1,math.abs(bnOffset.x) do
  560.         turtle.forward()
  561.       end
  562.     else
  563.       for moves = 1,math.abs(bnOffset.x) do
  564.         turtle.back()
  565.       end
  566.     end
  567.     if bnOffset.z < 0 then
  568.       turtle.turnLeft()
  569.       for moves = 1,math.abs(bnOffset.z) do
  570.         turtle.forward()
  571.       end
  572.       turtle.turnRight()
  573.     else
  574.       turtle.turnRight()
  575.       for moves = 1,math.abs(bnOffset.z) do
  576.         turtle.forward()
  577.       end
  578.       turtle.turnLeft()
  579.     end
  580.     if bnOffset.y < 0 then
  581.       for moves = 1,math.abs(bnOffset.y) do
  582.         turtle.down()
  583.       end
  584.     elseif bnOffset.y > 0 then
  585.       for moves = 1,math.abs(bnOffset.z) do
  586.         turtle.up()
  587.       end
  588.     end
  589.   elseif facingDirection == 4 then
  590.     if bnOffset.z < 0 then
  591.       for moves = 1,math.abs(bnOffset.z) do
  592.         turtle.forward()
  593.       end
  594.     elseif bnOffset.z > 0 then
  595.       for moves = 1,math.abs(bnOffset.z) do
  596.         turtle.back()
  597.       end
  598.     end
  599.     if bnOffset.x < 0 then
  600.       turtle.turnLeft()
  601.       for moves = 1,math.abs(bnOffset.x) do
  602.         turtle.forward()
  603.       end
  604.       turtle.turnRight()
  605.     elseif bnOffset.x > 0 then
  606.       turtle.turnRight()
  607.       for moves = 1,math.abs(bnOffset.x) do
  608.         turtle.forward()
  609.       end
  610.       turtle.turnLeft()
  611.     end
  612.     if bnOffset.y < 0 then
  613.       for moves = 1,math.abs(bnOffset.y) do
  614.         turtle.down()
  615.       end
  616.     elseif bnOffset.y > 0 then
  617.       for moves = 1,math.abs(bnOffset.z) do
  618.         turtle.up()
  619.       end
  620.     end
  621.   end
  622.  
  623. end
  624.  
  625. local function seekDestination(currentVector, targetVector)
  626.   local hDisplacement
  627.   local moveCount = 0
  628.   local startVector = currentVector
  629.   local parentVector = currentVector
  630.   log(string.format("Current X:%s, Current Y:%s, Current Z:%s, Target X:%s,Target Y:%s,Target Z:%s", tostring(currentVector.x), tostring(currentVector.y), tostring(currentVector.z), tostring(targetVector.x), tostring(targetVector.y), tostring(targetVector.z)))
  631.   while currentVector.x ~= targetVector.x and currentVector.z ~= targetVector.z and moveCount < 8 do
  632.     moveCount = moveCount + 1
  633.     log(string.format("Starting move #%d", moveCount))
  634.     log(string.format("Current X:%s, Current Y:%s, Current Z:%s, Target X:%s,Target Y:%s,Target Z:%s", tostring(currentVector.x), tostring(currentVector.y), tostring(currentVector.z), tostring(targetVector.x), tostring(targetVector.y), tostring(targetVector.z)))
  635.     local lastParentNode, nodeDataStatus = getNodeData(currentVector, targetVector, startVector, parentVector)
  636.     log(string.format("getNodeData returned %s", tostring(nodeDataStatus)))
  637.     local bestMoveVector = getBestNode(currentVector, targetVector , startVector)
  638.     log(string.format("getBestNode returned %s", tostring(bestMoveVector)))
  639.     if bestMoveVector then
  640.       log("Calculating BestMove Offset")
  641.       local bestMoveOffset = bestMoveVector - currentVector
  642.       log(string.format("Offset of %s beween bestmove:%s and current%s", tostring(bestMoveOffset),tostring(bestMoveVector), tostring(currentVector)))
  643.       local bmResult = gotoNode(bestMoveOffset, currentVector)
  644.       log(string.format("gotoNode returned %s", tostring(bmResult)))
  645.       parentVector = currentVector
  646.       log(string.format("Parent Vector set to %s", tostring(parentVector)))
  647.     else
  648.       log("No Best Node Returned")
  649.     end
  650.     currentVector = vector.new(gps.locate(5))
  651.     log(string.format("new current Vector set to %s", tostring(currentVector)))
  652.   end
  653. end
  654. --************Starting Seek*************
  655. nodeList = {}
  656. cVector = vector.new(gps.locate(5))
  657. sVector = cVector
  658. targetVector = vector.new(-12, 75,-562)
  659. m = seekDestination(cVector,targetVector,sVector )
RAW Paste Data