Modified moveto in preparation for circumnavigation.

This commit is contained in:
Harrison Deng 2021-05-05 19:23:58 -05:00
parent 825bc0b523
commit 1bdc320818

View File

@ -271,7 +271,6 @@ local function attemptStartTask(name, f, ...)
taskArgs = arg
end
-- Actions
local function determinePosition()
@ -366,7 +365,7 @@ local function moveBy(delta, forward, right)
end
end
local function moveTo(dest, range)
local function moveTo(dest)
sendLog(MOVETO, "Attempting to move by (" .. dest.x .. "," .. dest.y .. "," .. dest.z .. ") (x,y,z).")
if position == nil or heading == nil then
sendLog(FAILED, "Position or heading is invalid.")
@ -376,8 +375,8 @@ local function moveTo(dest, range)
local oldPos = nil
while not vectorEqual(dest, position) do
if oldPos ~= nil and vectorEqual(oldPos, position) then
sendLog(FAILED, "Cannot proceed to due obstacle.")
return
sendLog(FAILED, "Cannot proceed to due obstacle. XYZ being blocked: " .. xBlocked .. "," .. yBlocked .. "," .. zBlocked)
break
end
oldPos = vector.new(position.x, position.y, position.z);
if position.x ~= dest.x then
@ -385,13 +384,13 @@ local function moveTo(dest, range)
sendLog(FAILED, "Unable to orient for X axis.")
return
end
attemptMoveForward()
xblocked = attemptMoveForward()
end
if position.y ~= dest.y then
if dest.y - position.y > 0 then
attemptMoveUp()
yBlocked = attemptMoveUp()
else
attemptMoveDown()
yBlocked = attemptMoveDown()
end
end
if position.z ~= dest.z then
@ -399,7 +398,7 @@ local function moveTo(dest, range)
sendLog(FAILED, "Unable to orient for Z axis.")
return
end
attemptMoveForward()
zBlocked = attemptMoveForward()
end
end
end