View difference between Paste ID: Kkf2crjf and Xziv6utc

 local component = require 'component'
 local modem = component.modem
 
-local QUEEN = {}
+local PAWN = {}
 
 
 local FIRMWARE = {
 [[
-r= component.proxy(component.list('radar')())
+
-n= component.proxy(component.list('navigation')())
+
 Channel = 2413
 ResponseChannel = 2403
-Channel = 2412
+
-ResponseChannel = 2402
+
 m.open(Channel)
-m.setStrength(math.huge)
+
-drone_inv = "inv_q"
+
-isDroneQueen = true
+drone_inv = "inv_s"
 isDroneQueen = false
-broadcastGPS = false
+
 
-gpsRequestChannel = 20
+gpsSats={}
-m.open(gpsRequestChannel)
+cmdTRGPos={}
 ]]
 ,
 [[
 function sleep(timeout)
 	checkArg(1, timeout, 'number', 'nil')
 	local deadline = computer.uptime() + (timeout or 0)
 	repeat
 		computer.pullSignal(deadline - computer.uptime())
 	until computer.uptime() >= deadline
 end
 ]]
 ,
 [[
-function sub_vec(A,B) return {A[1]-B[1],A[2]-B[2],A[3]-B[3]} end
+floor, sqrt, abs = math.floor, math.sqrt, math.abs
-function math.trunc(v)
+
-    local t = math.modf(v)
+function round(v, m) return {x = floor((v.x+(m*0.5))/m)*m, y = floor((v.y+(m*0.5))/m)*m, z = floor((v.z+(m*0.5))/m)*m} end
-	return t
+function cross(v, b) return {x = v.y*b.z-v.z*b.y, y = v.z*b.x-v.x*b.z, z = v.x*b.y-v.y*b.x} end
 function len(v) return sqrt(v.x^2+v.y^2+v.z^2) end
 function dot(v, b) return v.x*b.x+v.y*b.y+v.z*b.z end
 function add(v, b) return {x=v.x+b.x, y=v.y+b.y, z=v.z+b.z} end
-		return {math.trunc(A[1]),math.trunc(A[2]),math.trunc(A[3])}
+function sub(v, b) return {x=v.x-b.x, y=v.y-b.y, z=v.z-b.z} end
 function mul(v, m) return {x=v.x*m, y=v.y*m, z=v.z*m} end
 function norm(v) return mul(v, 1/len(v)) end
 
 function arr_length(a)
   local c = 0
   for k,_ in pairs(a) do c=c+1 end
-function getPlayerCoord(e_name)
+  return c
-	checkArg(1,e_name,'string','nil')
+
-	for k,v in ipairs(r.getPlayers()) do
+function trunc(v) local t = math.modf(v) return t end
-		if v.name == e_name then
+
-			return {v.x,v.y,v.z},v.distance
+
-		end
+		return {x=trunc(A.x),y=trunc(A.y),z=trunc(A.z)}
 	end
 	return nil
 end
 ]]
 ,
 
-function move(x,y,z)
+
-	checkArg(1,x,'number','nil')
+function add2GPSTable(r_addr,x,y,z,dist)
-	checkArg(1,y,'number','nil')
+  if arr_length(gpsSats) < 7 then gpsSats[r_addr] = {x=x,y=y,z=z,d=dist} end
-	checkArg(1,z,'number','nil')
+
-	if x and y and z then
+
-			d.setLightColor(0x00FFAF)
+
-			d.move(x,y,z)
+
 function replyInv(add)
 	m.send(add,ResponseChannel,"stats",isFree,isDroneQueen)--Queens send "true"
 end
 ]]
 ,
 [[
 acts = {
 	[drone_inv] = function(r_add) d.setLightColor(0x00FFBB) replyInv(r_add) end,
 	["commit"] = function() d.setLightColor(0x5E00FF) isFree = false end,
 	["uncommit"] = function() isFree = true end,
 
 	["gps"] = function(r_addr,x,y,z,dist) add2GPSTable(r_addr,x,y,z,dist) end,
-function QgpsBroadcast(c)
+	["trg"] = function(_,x,y,z,dist) cmdTRGPos={c={x,y,z},d=dist} end,
-	if broadcastGPS and c[1] then
+	["formup"] = function(_,x,y,z,_,trgC) d.setStatusText(gpsMoveToTarget({x=x,y=y,z=z},trgC)) end,
-		--m.broadcast(gpsChannel,"gps",-c[1],-(c[2]+1),-c[3])
+	["orbit"] = function(_,x,y,z,_,trgC) d.setStatusText("orbiting") d.setStatusText(gpsOrbitTRG({x=x,y=y,z=z},trgC)) end,
-		m.broadcast(gpsChannel,"gps",c[1],c[2],c[3])
+
-		d.setLightColor(0x00FFCC)
+
 ]]
 ,
 [[
 actsWhileMoving = {
 	[drone_inv] = function(r_add) replyInv(r_add) end,
-function gpsReply(addr)
+
-	if gpsSatPos[1] then
+
-		m.send(addr,gpsChannel,"gps",gpsSatPos[1],gpsSatPos[2],gpsSatPos[3])
+	["gps"] = function(r_addr,x,y,z,dist) add2GPSTable(r_addr,x,y,z,dist) end,
 	--["trg"] = function(_,x,y,z,dist) cmdTRGPos={c={x=x,y=y,z=z},d=dist} d.setStatusText("trg2:"..tostring(cmdTRGPos.c.x))  end,
 	["HUSH"] = function() d.setLightColor(0xFF0000) sleep(1) computer.shutdown() end
 }
 ]]
 ,
 [[
-	["go"] = function(_,tag) d.setLightColor(0x00FF00) d.setStatusText(navMoveToPlayer(tag)) end,
+function trilaterate(A, B, C)
-    ["bzz"] = function(_,tag) d.setLightColor(0x0000FF) d.setStatusText(navSwarmPlayer(tag)) end,
+	local a2b = {x=B.x-A.x, y=B.y-A.y, z=B.z-A.z}
-	["move"] = function(_,_,x,y,z) move(x,y,z) end,
+	local a2c = {x=C.x-A.x, y=C.y-A.y, z=C.z-A.z}
-	[drone_inv] = function(r_add) d.setLightColor(0xFF00BB) replyInv(r_add) end,
+	if abs(dot(norm(a2b), norm(a2c))) > 0.999 then return nil end
 	local d, ex = len(a2b), norm(a2b)
 	local i = dot(ex, a2c)
-	["formup"] = function(_,tag,x,y,z) d.setStatusText(navMoveToPlayerWOffset(tag,x,y,z)) end,
+	local exMi = mul(ex, i)
-	["setgpspos"] = function(_,_,x,y,z) gpsSatPos = {x,y,z} end,
+	local ey = norm(sub(a2c, mul(ex, i)))
-	["startgps"] = function() broadcastGPS = true end,
+	local j, ez = dot(ey, a2c), cross(ex, ey)
-	["stopgps"] = function() broadcastGPS = false end,
+	local r1, r2, r3 = A.d, B.d, C.d
-	["reqgps"] = function(r_add) gpsReply(r_add) end,
+	local x = (r1^2 - r2^2 + d^2) / (2*d)
 	local y = (r1^2 - r3^2 - x^2 + (x-i)^2 + j^2) / (2*j)
 	local result = add(A, add(mul(ex, x), mul(ey, y)))
 	local zSquared = r1^2 - x^2 - y^2
 	if zSquared > 0 then
 		local z = sqrt( zSquared )
 		local result1 = add(result, mul(ez, z))
 		local result2 = sub(result, mul(ez, z))
 		local rnd1, rnd2 = result1,result2
 		if rnd1.x ~= rnd2.x or rnd1.y ~= rnd2.y or rnd1.z ~= rnd2.z then
-	["setgpspos"] = function(_,_,x,y,z) gpsSatPos = {x,y,z} end,
+			return rnd1, rnd2
-	["startgps"] = function() broadcastGPS = true end,
+
-	["stopgps"] = function() broadcastGPS = false end,
+			return rnd1
 		end
 	end
 	return result
 end
 ]]
-function navMoveToPlayer(e_name)
+
-	checkArg(1,e_name,"string","nil")
+
-	local trgPos = {n.getPosition()},msg,r_add
+function narrow(p1, p2, fix)
-
+	local d1 = abs(len(sub(p1, fix))-fix.d)
-	if not trgPos[1] then d.setLightColor(0xFF0000) return "Out Of\nRange" end
+	local d2 = abs(len(sub(p2, fix))-fix.d)
-
+	if abs(d1-d2) < 0.01 then
-	trgPos = vec_trunc(trgPos)
+		return p1, p2
-	local mv = {0,0,0}
+	elseif d1 < d2 then
-	local mapRange = n.getRange()
+		return p1,nil
-
+	else
 		return p2,nil
-		local v = getPlayerCoord(e_name)
+
-		if not v then
+
-			d.setLightColor(0xFF0000)
+
-			d.setStatusText("Out Of\nRange")
+
 [[
-			v = vec_trunc(v)
+function getGPSlocation()
-			local npos = {n.getPosition()}
+	local fixes = {}
-			if npos[1] then
+	local pos1, pos2 = nil, nil
-				npos = vec_trunc(npos)
+	for addr,fix in pairs(gpsSats) do
-				local Qpn = {npos[1] + v[1], npos[2] + v[2] + 2, npos[3] + v[3]}
+		if fix.d == 0 then
-				mv = sub_vec(Qpn,trgPos)
+			pos1, pos2 = {x=fix.x, y=fix.y, z=fix.z}, nil
-				if math.abs(Qpn[1]) < mapRange-5 and math.abs(Qpn[3]) < mapRange-5 then
+		else
-					d.move(mv[1],mv[2],mv[3])
+			table.insert(fixes, fix)
-					trgPos = Qpn
+
 	end
-			else
+	if #fixes >= 3 then
-				d.setLightColor(0xFF0000)
+		if not pos1 then
-				d.setStatusText("Out Of\nRange")
+			pos1, pos2 = trilaterate(fixes[1], fixes[2], fixes[3])
-				d.move(-mv[1],-mv[2],-mv[3])
+
 		if pos1 and pos2 then
 			for f=4,#fixes do
-		_,_,r_add,_,_,msg = computer.pullSignal(0.5)
+				pos1, pos2 = narrow(pos1, pos2, fixes[f])
 				if pos1 and not pos2 then break end
-			actsWhileMoving[msg](r_add)
+
 		end
-	until msg == "stop"
+	end
 
 	if pos1 and pos2 then
 		d.setStatusText("ps12")
 		return nil
 	elseif pos1 then
-function navSwarmPlayer(e_name)
+		local c = round(pos1,1)
-	checkArg(1,e_name,"string","nil")
+		return {x=c.x,y=c.y,z=c.z}
-	local trgPos = {n.getPosition()},msg,r_add
+	else
-
+		d.setStatusText("else")
-	if not trgPos[1] then d.setLightColor(0xFF0000) return "Out Of\nRange" end
+		return nil
-
+
-	trgPos = vec_trunc(trgPos)
+
-	local mv = {0,0,0}
+
-	local mapRange = n.getRange()
+
-
+
 refreshGPSInterval = 0
-		local v = getPlayerCoord(e_name)
+function refreshGPSTable()
-		if not v then
+	if refreshGPSInterval >= 17 then gpsSats={} refreshGPSInterval = 0 end
-			d.setLightColor(0xFF0000)
+	refreshGPSInterval = refreshGPSInterval + 1
-			d.setStatusText("Out Of\nRange")
+
 
-			v = vec_trunc(v)
+function getTRGPos()
-			local npos = {n.getPosition()}
+	return cmdTRGPos
-			if npos[1] then
+
-				npos = vec_trunc(npos)
+
-				local Qpn = {npos[1] + v[1] +math.random(-3,3), npos[2] + v[2] +math.random(-3,3), npos[3] + v[3]+math.random(-3,3)}
+
-				mv = sub_vec(Qpn,trgPos)
+
-				if math.abs(Qpn[1]) < mapRange-5 and math.abs(Qpn[3]) < mapRange-5 then
+function gpsMoveToTarget(offset,trgChannel)
-					d.move(mv[1],mv[2],mv[3])
+	d.setLightColor(0xFFFFFF)
-					trgPos = Qpn
+	gpsSats={}
 	m.open(gpsChannel)
-			else
+	local ctrlTRGPos = nil
-				d.setLightColor(0xFF0000)
+
-				d.setStatusText("Out Of\nRange")
+		if arr_length(gpsSats)>=3 then
-				d.move(-mv[1],-mv[2],-mv[3])
+			ctrlTRGPos = getGPSlocation()
 		end
 
-		_,_,r_add,_,_,msg = computer.pullSignal(0.5)
+		if ctrlTRGPos then ctrlTRGPos = vec_trunc(ctrlTRGPos)
 		else d.setLightColor(0xFF0000) d.setStatusText("No GPS:") end
-			actsWhileMoving[msg](r_add)
+
 		_,_,r_addr,_,dist,msg,x,y,z,trgCh = computer.pullSignal(0.5)
-	until msg == "stop"
+
 		if actsWhileMoving[msg] then
 			actsWhileMoving[msg](r_addr,x,y,z,dist)
 		end
 		refreshGPSTable()
 	until msg == "stop" or ctrlTRGPos
-gpsSatPos={}
+
-function navMoveToPlayerWOffset(e_name,xo,yo,zo)
+	if ctrlTRGPos then
-	checkArg(1,e_name,"string","nil")
+		m.close(gpsChannel)
-	local trgPos = {n.getPosition()}
+		d.setLightColor(0xFFFFFF)
-	if not trgPos[1] then d.setLightColor(0xFF0000) return "Out Of\nRange" end
+		m.open(trgChannel)
-	trgPos = vec_trunc(trgPos)
+		local mv = {x=0,y=0,z=0},msg,r_add,dist,x,y,z
-	local mv = {0,0,0},msg,r_add
+		local trgUpdate = {}
-	local mapRange = n.getRange()
+			repeat
 				_,_,r_addr,_,dist,msg,x,y,z,trgCh = computer.pullSignal(0.5)
-		local v = getPlayerCoord(e_name)
+
-		if not v then
+				if msg == "trg" then
-			d.setLightColor(0xFF0000)
+					trgUpdate = {c={x=x,y=y,z=z},d=dist}
-			d.setStatusText("Out Of\nRange")
+
 				local trgPos = trgUpdate
-			v = vec_trunc(v)
+
-			--QgpsBroadcast(v)
+				if trgPos.d and trgPos.d < 50 then
 					trgPos.c = vec_trunc(trgPos.c)
-			--gpsSatPos = {-v[1],-(v[2]+1),-v[3]} --offset origin to tablet position (above player's head)
+					local trgPosOffset = add(trgPos.c, offset)
-			--gpsSatPos = {-v[1],-v[2],-v[3]}
+					mv = sub(trgPosOffset,ctrlTRGPos)
 					d.move(mv.x,mv.y,mv.z)
-			local npos = {n.getPosition()}
+					ctrlTRGPos = trgPosOffset
-			if npos[1] then
+					d.setLightColor(0x00FF00)
-				npos = vec_trunc(npos)
+					d.setStatusText(d.name())
-				local Qpn = {npos[1] + v[1] + xo, npos[2] + v[2] + yo, npos[3] + v[3] + zo}
+				else
-				mv = sub_vec(Qpn,trgPos)
+					d.setLightColor(0xFF0000)
-				if math.abs(Qpn[1]) < mapRange-5 and math.abs(Qpn[3]) < mapRange-5 then
+					d.setStatusText("Out Of\nRange")
-					d.move(mv[1],mv[2],mv[3])
+					d.move(-mv.x,-mv.y,-mv.z)
-					trgPos = Qpn
+
 				if actsWhileMoving[msg] then
-			else
+					actsWhileMoving[msg](r_addr,x,y,z,dist)
-				d.setLightColor(0xFF0000)
+
-				d.setStatusText("Out Of\nRange")
+			until msg == "stop"
-				d.move(-mv[1],-mv[2],-mv[3])
+
 	m.close(trgChannel)
 	return d.name()
-		_,_,r_add,_,_,msg = computer.pullSignal(0.5)
+
 ]]
-			actsWhileMoving[msg](r_add)
+
 [[
-	until msg == "stop"
+function rotatePoint(rad,point)
 	local s = math.sin(rad);
 	local c = math.cos(rad);
 
 	// rotate point
 	float xnew = point.x * c - point.y * s;
 	float ynew = point.x * s + point.y * c;
-local cmd,tag,x,y,z
+
-d.setLightColor(0xFFAF00)
+	return point
 end
-	_,_,r_add,_,_,cmd,tag,x,y,z = computer.pullSignal(0.5)
+
-	if d.name():match("^Q%d+$") then
+
-		if acts[cmd] then
+
-			acts[cmd](r_add,tag,x,y,z)
+function gpsOrbitTRG(offset,trgChannel)
 	d.setLightColor(0xFFFFFF)
-		QgpsBroadcast(gpsSatPos)
+	gpsSats={}
 	m.open(gpsChannel)
 	local ctrlTRGPos = nil
 	repeat
 		if arr_length(gpsSats)>=3 then
 			ctrlTRGPos = getGPSlocation()
 		end
 
-function QUEEN.broadcastFirmWare(port)
+		if ctrlTRGPos then ctrlTRGPos = vec_trunc(ctrlTRGPos)
 		else d.setLightColor(0xFF0000) d.setStatusText("No GPS:") end
 
 		_,_,r_addr,_,dist,msg,x,y,z,trgCh = computer.pullSignal(0.5)
 
 		if actsWhileMoving[msg] then
-return QUEEN
+			actsWhileMoving[msg](r_addr,x,y,z,dist)
 		end
 		refreshGPSTable()
 	until msg == "stop" or ctrlTRGPos
 
 	if ctrlTRGPos then
 		m.close(gpsChannel)
 		d.setLightColor(0xFFFFFF)
 		m.open(trgChannel)
 		local mv = {x=0,y=0,z=0},msg,r_add,dist,x,y,z
 		local trgUpdate = {}
 		local currentAngle = 0 -- in radians
 		local rotationInterval = 0.785 -- PI/4
 			repeat
 				_,_,r_addr,_,dist,msg,x,y,z,trgCh = computer.pullSignal(0.5)
 
 				if msg == "trg" then
 					trgUpdate = {c={x=x,y=y,z=z},d=dist}
 				end
 				local trgPos = trgUpdate
 
 				if trgPos.d and trgPos.d < 50 then
 					trgPos.c = vec_trunc(trgPos.c)
 
 					if currentAngle >= 6.3832 then currentAngle = 0 end
 					local rotatedOffset = rotatePoint(currentAngle,offset)
 					currentAngle = currentAngle + rotationInterval
 
 					local trgPosOffset = add(trgPos.c, rotatedOffset)
 
 					mv = sub(trgPosOffset,ctrlTRGPos)
 					d.move(mv.x,mv.y,mv.z)
 					ctrlTRGPos = trgPosOffset
 					d.setLightColor(0x00FF00)
 					d.setStatusText(d.name())
 				else
 					d.setLightColor(0xFF0000)
 					d.setStatusText("Out Of\nRange")
 					d.move(-mv.x,-mv.y,-mv.z)
 				end
 				if actsWhileMoving[msg] then
 					actsWhileMoving[msg](r_addr,x,y,z,dist)
 				end
 			until msg == "stop"
 	end
 	m.close(trgChannel)
 	return d.name()
 end
 ]]
 ,
 [[
 d.setAcceleration(100)
 d.setLightColor(0x007B62)
 while true do
 	_,_,r_addr,_,dist,msg,x,y,z,trgCh = computer.pullSignal(0.5)
 	if d.name():match("^S%d+$") then
 		if msg =="orbit"then d.setStatusText("orbitter") end
 		if acts[msg] then
 			acts[msg](r_addr,x,y,z,dist,trgCh)
 		end
 	end
 	--d.setLightColor(0xFFAF00)
 	--d.setLightColor(0x77FF77)
 end
 ]]
 }
 
 PAWN.master = "" --control tablet modem address, set before broadcasting firmware
 
 function PAWN.broadcastFirmWare(port)
 	modem.broadcast(port,
 	[[
 		master = ]]..PAWN.master..[[
 	]]
 	)
 	for _,part in ipairs(FIRMWARE) do
 		modem.broadcast(port,part)
 	end
 end
 
 return PAWN