View difference between Paste ID: WukTpheC and PhLMDtaN
SHOW: | | - or go back to the newest paste.
1
local component = require("component")
2
local robot = require("robot")
3
local computer = require("computer")
4
local nav = component.navigation
5
6
local wp = {}
7
8
-- color codes for the light
9
local lightError = 0xFF0000		-- error = red
10
local lightIdle = 0x0000FF		-- idle = blue
11
local lightCharge = 0xFFFFFF		-- charging = white
12
local lightMove = 0x00FF00		-- moving = green
13
local lightWork = 0xFFFF00		-- working = yellow (dig, drop, suck, etc.)
14
local lightBusy = 0xFF00FF		-- busy = magenta
15
16
-- the range that is used for finding findWaypoints
17
local scanRange = 256
18
19
-- by default robot will not auto charge 
20
local autoCharge = false
21
-- percentage robot will go to charger
22
local autoChargeValue = 0.10
23
-- stores name of the waypoint with charger for auto charging
24
local autoCharger = "autocharger"
25
26
-- Function writes an error message to stderr and changes the robot light to red
27
local function errorReport(msg)
28
  robot.setLightColor(lightError)
29
  io.stderr:write("[ERROR] " .. msg .. "\n")
30
end
31
32
--robot.setLightColor(lightBusy)
33
34
-- get the direction robot is looking
35
--2 north
36
--3 south
37
--4 west
38
--5 east
39
local facingTable = {}
40
facingTable[1] = "facings"
41
facingTable[2] = "north"
42
facingTable[3] = "south"
43
facingTable[4] = "west"
44
facingTable[5] = "east"
45
46
-- stores the height the robot is moving
47
local yPosition = 0 
48
49
-- facing the robot into the given position
50
local function face(direction)
51
  robot.setLightColor(lightMove)
52
  while facingTable[nav.getFacing()] ~= direction do
53
    robot.turnRight()
54
  end
55
  robot.setLightColor(lightIdle)
56
end
57
58
-- enables/disables auto charging
59
function wp.enableAutoCharge(value)
60
  autoCharge = value
61
end
62
63
-- returns name of the waypoint with charger
64
function wp.getAutoCharger()
65
  return autoCharger
66
end
67
68
-- set the name of the waypoint used for charging
69
function wp.setAutoCharger(value)
70
  autoCharger = value
71
end
72
73
-- Returns the range of findWaypoints
74
function wp.getScanRange()
75
  return scanRange
76
end
77
78
-- sets the range of findWaypoints
79
function wp.setScanRange(value)
80
  scanRange = value
81
end
82
83
-- moves robot to the given waypoint
84
local function gotoWaypoint(name, f)
85
    robot.setLightColor(lightBusy)
86
    local w = nav.findWaypoints(scanRange)
87
    if w == nil then
88
      errorReport("Way point " .. name .. " not found")
89
      return false
90
    end
91
    for k,v in pairs(w) do
92
      if k ~= "n" then
93
	if v.label == name then
94
	  x = math.floor(v.position[1])
95
	  y = math.floor(v.position[2])
96
	  z = math.floor(v.position[3])
97
	end
98
      end
99
    end
100
  print("[INFO] moving to " ..name .. " at " .. x .. "," .. y .. "," .. z)
101
  local steps = 0
102
  
103
  -- first the x axis (east - west)
104
  -- ---------------------------------
105
  -- calc the steps to moves
106
  steps = math.abs(x)
107
  
108
  if x > 0 then
109
    robot.setLightColor(lightMove)
110
    -- getting up first
111-
    for i=1, 4 do			-- getting 4 block high
111+
    for i=1, 1 do			-- getting 1 block high
112
      robot.up()
113
      yPosition = yPosition + 1
114
    end
115
    -- moving to east
116
    face("east")
117
    robot.setLightColor(lightMove)
118
    for i=1, steps do
119
      robot.forward()
120
    end
121
    robot.setLightColor(lightIdle)
122
  elseif x < 0 then
123
    robot.setLightColor(lightMove)
124
    -- getting up first
125-
    for i=1, 5 do			-- getting 5 blocks high
125+
    for i=1, 1 do			-- getting 1 blocks high
126
      robot.up()
127
      yPosition = yPosition + 1
128
    end
129
    -- moving to west
130
    face("west")
131
    robot.setLightColor(lightMove)
132
    for i=1, steps do
133
      robot.forward()
134
    end
135
    robot.setLightColor(lightIdle)
136
  end
137
  
138
  -- second the z axis (north - south)
139
  -- ---------------------------------
140
  -- calc the steps to moves
141
  steps = math.abs(z)
142
143
  -- set the facing
144
  if z > 0 then
145
    robot.setLightColor(lightMove)
146
    -- getting up first
147-
    for i=1, 2 do			-- getting 2 blocks extra high
147+
    for i=1, 1 do			-- getting 1 blocks extra high
148
      robot.up()
149
      yPosition = yPosition + 1
150
    end
151
    -- moving to south
152
    face("south")
153
    robot.setLightColor(lightMove)
154
    for i=1, steps do
155
      robot.forward()
156
    end
157
    robot.setLightColor(lightIdle)
158
  elseif z < 0 then
159
    robot.setLightColor(lightMove)
160
    -- getting up first
161-
    for i=1, 2 do			-- getting 2 blocks extra high
161+
    for i=1, 1 do			-- getting 1 blocks extra high
162
      robot.up()
163
      yPosition = yPosition + 1
164
    end
165
    -- moving to north
166
    face("north")
167
    robot.setLightColor(lightMove)
168
    for i=1, steps do
169
      robot.forward()
170
    end
171
    robot.setLightColor(lightIdle)
172
  end
173
  
174
  -- at last the height
175
  -- ---------------------------------
176
    
177
  if yPosition > y then
178
    robot.setLightColor(lightMove)
179
    steps = yPosition - math.floor(y)
180
    for i=1, steps do
181
      robot.down()
182
    end
183
    robot.setLightColor(lightIdle)
184
  elseif yPosition < y then
185
    robot.setLightColor(lightMove)
186
    steps = math.floor(y) -yPosition
187
    for i=1, steps do
188
      robot.up()
189
    end
190
    robot.setLightColor(lightIdle)
191
  end
192
  yPosition = 0
193
  
194
  -- if facing is set, we have to set it
195
  if f then
196
    face(f)
197
  end
198
end
199
200
-- the real function to move to a given waypoint. sends robot to charger, if autocharging is enabled
201
function wp.goTo(name,f)
202
  if autoCharge == true then
203
    if computer.energy() < (computer.maxEnergy()*autoChargeValue) then
204
      gotoWaypoint(autoCharger)
205
      robot.setLightColor(lightCharge)
206
      while computer.energy() < (computer.maxEnergy()*0.95) do		-- autocharge until 95% full
207
	os.sleep(0.2)
208
      end
209
      robot.setLightColor(lightIdle)
210
    end
211
  end
212
  gotoWaypoint(name,f)
213
end
214
215
return wp