nav.lua

rednet.open('back')
x = 0
y = 0
tolerance = 25
conv = 57.2958

function getGPS()
    local x, y, z = gps.locate(5)
    return x, z
end

function check(x)
    x = x + 5
end

function getPoint(str)
    rednet.broadcast(str, 'navSys')
    local ip, tempX = rednet.receive('navSys', 3)
    local ip, tempZ = rednet.receive('navSys', 3)
    return tempX, tempZ
end

function getDistance(a, b)
    if a == 'self' then x1, z1 = getGPS()
    else x1, z1 = getPoint(a) end
    if check(x1) then return 'error' end
    x2, z2 = getPoint(b)
    x = 0 + (x1 - x2)
    z = 0 - (z1 - z2)
    h = math.floor(((x*x) + (z*z))^0.5+0.5)
    return h
end

function getAngle(a, b)
    if a == 'self' then x1, z1 = getGPS()
    else x1, z1 = getPoint(a) end
    if check(x1) then return 'error' end
    x2, z2 = getPoint(b)
    x = 0 + (x1 - x2)
    z = 0 - (z1 - z2)
    local num = math.floor((conv * math.atan(x/z) + 0.5))
    if z > 0 then num = num + 180 end
    if num < 1 then num = num + 360
    elseif num > 360 then num = num - 360 end
    return num
end

waypoints = {}
wp = ""
while wp ~= "END" do
    wp = read()
    if wp ~= "END" then
        table.insert(waypoints, wp)
    end
end

for i=1, #waypoints do
    if i ~= 1 then
        angle = getAngle(waypoints[i-1], waypoints[i])
        distance = getDistance(waypoints[i-1], waypoints[i])
        if string.len(waypoints[i]) == 7 then
            str = '  | '..distance..'m, '..angle..'DEG'
        else
            str = ' | '..distance..'m, '..angle..'DEG'
        end
        print(i..': '..waypoints[i]..str)
    else
        print(i..': '..waypoints[i])
    end
end
read()


i = 1
while i <= #waypoints do
    a = getAngle('self', waypoints[i])
    d = getDistance('self', waypoints[i])
    term.clear()
    term.setCursorPos(1, 1)
    print('NEXT WPT: '..waypoints[i])
    if a == 'error' or d == 'error' then
        print('Unable to locate GPS')
        sleep(1)
    else
        print(d..'m, '..a..'DEG')
        if d <= tolerance then i = i+1 end
    end
end