vx13

lib/erobot.lua

Feb 12th, 2017
213
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. local component = require("component")
  2. local sides = require("sides")
  3.  
  4. local robot = require("robot")
  5.  
  6. -------------------------------------------------------------------------------
  7. -- Navigation
  8.  
  9. -- Minecraft
  10. --
  11. --            z
  12. --           (-)
  13. --          north
  14. -- x (-) west   east (+) x
  15. --          south
  16. --           (+)
  17. --            z
  18. --         forward
  19.  
  20. -- Virtual
  21. --
  22. --         forward
  23. --            z
  24. --           (+)
  25. --          -----
  26. -- x (-) ---- ^ ---- (+) x
  27. --          -----
  28. --           (-)
  29. --            z
  30.  
  31. local orientation = sides.east
  32. local x, y, z = 0, 0, 0
  33. local startOrientation = orientation
  34. local startX, startY, startZ = x, y, z
  35.  
  36. local clockwiseSide = {
  37.     [sides.south] = sides.west,
  38.     [sides.west] = sides.north,
  39.     [sides.north] = sides.east,
  40.     [sides.east] = sides.south,
  41. }
  42.  
  43. local counterclockwiseSide = {
  44.     [sides.south] = sides.east,
  45.     [sides.east] = sides.north,
  46.     [sides.north] = sides.west,
  47.     [sides.west] = sides.south,
  48. }
  49.  
  50. local defMove = component.robot.move
  51. local defTurn = component.robot.turn
  52.  
  53. function navTurn(clockwise)
  54.   local ok, err = defTurn(clockwise)
  55.   if ok then
  56.     if clockwise then
  57.       orientation = clockwiseSide[orientation]
  58.     else
  59.       orientation = counterclockwiseSide[orientation]
  60.     end
  61.   end
  62.   return ok, err
  63. end
  64.  
  65. local hmover = {
  66.     [sides.south] = function(n) z = z + n end,
  67.     [sides.north] = function(n) z = z - n end,
  68.     [sides.east] = function(n) x = x + n end,
  69.     [sides.west] = function(n) x = x - n end,
  70. }
  71.  
  72. local mover = {
  73.   [sides.forward] = function() hmover[orientation](1) end,
  74.   [sides.back] = function () hmover[orientation](-1) end,
  75.   [sides.up] = function () y = y + 1 end,
  76.   [sides.down] = function () y = y - 1 end,
  77. }
  78.  
  79. function navMove(side)
  80.   local ok, err
  81.   while true do
  82.     ok, err =  defMove(side)
  83.     if not ok and err == "already moving" then
  84.       os.sleep(0.5)
  85.     else
  86.       break
  87.     end
  88.   end
  89.   if ok then
  90.     local fn = mover[side]
  91.     assert(fn, "internal error")
  92.     fn()
  93.   end
  94.   return ok, err
  95. end
  96.  
  97. component.robot.move = navMove
  98. component.robot.turn =navTurn
  99.  
  100. function robot.navInit(side, start_x, start_y, start_z)
  101.   checkArg(1, side, "nil", "number")
  102.   checkArg(2, start_x, "nil", "number")
  103.   checkArg(3, start_y, "nil", "number")
  104.   checkArg(4, start_z, "nil", "number")
  105.   side = side or sides.east
  106.   start_x = start_x or 0
  107.   start_y = start_y or 0
  108.   start_z = start_z or 0
  109.   assert(side >= sides.north and side <= sides.east, "Bad orientation argument.") -- see sides
  110.   startX, startY, startZ = start_x, start_y, start_z
  111.   startOrientation = side
  112.   x, y, z = startX, startY, startZ
  113.   orientation = startOrientation
  114. end
  115.  
  116. function robot.getOrientation()
  117.   return orientation
  118. end
  119.  
  120. function robot.getPos()
  121.   return x, y, z
  122. end
  123.  
  124. -- условная система координат
  125. --
  126. --
  127. --      (+x)
  128. -- (-z)  ^   (+z)
  129. --      (-x)
  130. -- ^ стартовая ориентация
  131. -- отсчёт идёт от точки старта
  132.  
  133. local toVirtFn = {
  134.   [sides.east] = function(realX, realY, realZ) return   realX - startX, realY - startY, realZ - startZ end,
  135.   [sides.south] = function(realX, realY, realZ) return  realZ - startZ, realY - startY, startX - realX end,
  136.   [sides.west] = function(realX, realY, realZ) return   startX - realX, realY - startY, startZ - realZ end,
  137.   [sides.north] = function(realX, realY, realZ) return  startZ - realZ, realY - startY, realX - startX end,
  138. }
  139.  
  140. function robot.toVirtCoord(realX, realY, realZ)
  141.   return toVirtFn[startOrientation](realX, realY, realZ)
  142. end
  143.  
  144. --            z
  145. --           (-)
  146. --          north
  147. -- x (-) west   east (+) x
  148. --          south
  149. --           (+)
  150. --            z
  151. --         forward
  152.  
  153. local toRealFn = {
  154.   [sides.east] = function(virtX, virtY, virtZ) return   startX + virtX, startY + virtY, startZ + virtZ end,
  155.   [sides.south] = function(virtX, virtY, virtZ) return  startX - virtZ, startY + virtY, startZ + virtX end,
  156.   [sides.west] = function(virtX, virtY, virtZ) return   startX - virtX, startY + virtY, startZ - virtZ end,
  157.   [sides.north] = function(virtX, virtY, virtZ) return  startX + virtZ, startY + virtY, startZ - virtX end,
  158. }
  159.  
  160. function robot.toRealCoord(virtX, virtY, virtZ)
  161.   return toRealFn[startOrientation](virtX, virtY, virtZ)
  162. end
  163.  
  164. function robot.getStartOrientation()
  165.   return startOrientation
  166. end
  167.  
  168. function robot.getStartPos()
  169.   return startX, startY, startZ
  170. end
  171.  
  172. function robot.rotateTo(side)
  173.   checkArg(1, side, "number")
  174.   assert(side >= sides.north and side <= sides.east, "Bad side argument.") -- see sides
  175.   if side == orientation then
  176.     return true
  177.   end
  178.   local clockwise = (clockwiseSide[orientation] == side)
  179.   local n = 1
  180.   if not clockwise and (counterclockwiseSide[orientation] ~= side) then
  181.     n = 2
  182.   end
  183.   for i = 1, n do
  184.     local ok, err = navTurn(clockwise)
  185.     if not ok then
  186.       return false, err
  187.     end
  188.   end
  189.   return true
  190. end
  191.  
  192. function robot.moveTo(targetX, targetY, targetZ, mover)
  193.   checkArg(1, targetX, "number")
  194.   checkArg(2, targetY, "number")
  195.   checkArg(3, targetZ, "number")
  196.   checkArg(4, action, "nil", "function")
  197.   mover = mover or component.robot.move
  198.   if x ~= targetX then
  199.     if targetX < x then
  200.       robot.rotateTo(sides.west)
  201.     else
  202.       robot.rotateTo(sides.east)
  203.     end
  204.     while x ~= targetX do
  205.       local ok, err = mover(sides.forward)
  206.       if not ok then
  207.         return false, err
  208.       end
  209.     end
  210.   end
  211.   if z ~= targetZ then
  212.     if targetZ < z then
  213.       robot.rotateTo(sides.north)
  214.     else
  215.       robot.rotateTo(sides.south)
  216.     end
  217.     while z ~= targetZ do
  218.       local ok, err = mover(sides.forward)
  219.       if not ok then
  220.         return false, err
  221.       end
  222.     end
  223.   end
  224.   if y ~= targetY then
  225.     local dir
  226.     if targetY < y then
  227.       dir = sides.down
  228.     else
  229.       dir = sides.up
  230.     end
  231.     while y ~= targetY do
  232.       local ok, err = mover(dir)
  233.       if not ok then
  234.         return false, err
  235.       end
  236.     end
  237.   end
  238.   return true
  239. end
  240.  
  241. function robot.moveByPath(path, pos, mover)
  242.   for i, p in ipairs(path) do
  243.     if i >= pos then
  244.       pos = i
  245.       local ok, err = robot.moveTo(p.x, p.y, p.z, mover)
  246.       if not ok then
  247.         return false, pos, err
  248.       end
  249.     end
  250.   end
  251.   return true, pos
  252. end
  253.  
  254. -------------------------------------------------------------------------------
  255.  
  256. return robot
RAW Paste Data