Cleaning robot [AP]

local detector = peripheral.find("environmentDetector")
local location = {x=0,y=0,z=0}
local facing = 0 --0=North, 1=East, 2=South, 3=West

--Parameters
local radius = 15
local base = {x=0,y=0,z=0}


function CorrectRotation(expected)
    if (facing~=expected) then
        if (facing-1==expected) then
            turtle.turnLeft()
        else
            for i=facing,expected,1 do
                turtle.turnRight()
            end
        end

    end
end

function MoveDirection(direction, amount)
    CorrectRotation(direction)
    for i=1,amount,1 do
        turtle.forward()
        turtle.suck()
    end
end

function GoTo(x,y,z)
    --stop right before arriving
    while(true) do
        if (x~=location.x) then
            if (x>location.x) then
                MoveDirection(0, x-location.x)
            else
                MoveDirection(2, location.x-x)
            end
        elseif (y~=location.y) then
            if (y>location.y) then
                turtle.up()
            else
                turtle.down()
            end
        elseif (z~=location.z) then
            if (z>location.z) then
                MoveDirection(1, z-location.z)
            else
                MoveDirection(3, location.z-z)
            end
        end
    end
end

while(true) do

    local entities = detector.scanEntities(radius)
    if (entities ~= nil) then
        for i,ent in ipairs(entities) do

            if (true) then
                GoTo(ent.x,ent.y,ent.z)
                GoTo(base.x, base.y, base.z)
            end

        end
    end

    sleep(2)
end