movement.lua

local movement={}
local robot = require("robot")

-- Angles: 1=North; 2=West; 3=South; 4=East (Relative to robot placement! Faceing when placed will allways be North!).
local angle = 1

-- tries to move the Robot forward by @times times, if there is a obstacle, swing the current Tool.
function movement.tryForward(times)
  for i=1, times do
    while not robot.forward() do
      robot.swing()
    end
  end
end

-- tries to move the Robot upwards by @times times, if there is an obstacle, swing the current Tool.
function movement.tryUp(times)
  for i=1, times do
    while not robot.up() do
      robot.swingUp()
    end
  end
end

--tries to move the Robot downwards by @times times, if there is an obstacle, swing the current Tool.
function movement.tryDown(times)
  for i=1, times do
    while not robot.down() do
      robot.swingDown()
    end
  end
end

-- turns the robot by @degree times in the Direction of @dir<1=Left; 2=Right>
-- Possible degrees: 1=90°; 2=180°; 3=270°; 4=360°; 5=450°...
-- keeps track of the robots current Angle.
function movement.turn(degree, dir)
  local Degree = degree
  local Direction = dir
  for i=1, Degree do
    if Direction==1 then
      robot.turnLeft()
      if angle<4 then
        angle=angle+1
      else
        angle=1
      end
    end
    if Direction==2 then
      robot.turnRight()
      if angle>1 then
        angle=angle-1
      else
        angle=4
      end
    end
  end
  print(angle)
end


function movement.returnHome(posX, posY, posZ, pointX, pointY, pointZ)
  local diffX = posX-pointX
  local diffY = posY-pointY
  local diffZ = posZ-pointZ

  gotoY(diffY)
  sleep(1)
  turnToAngle(4)
  gotoXZ(diffZ)
  sleep(1)
  turnToAngle(3)
  gotoXZ(diffX)
end

-- Goes forward a given amount of @times. Warning! Does not set the Angle automatically!
local function gotoXZ(times)
  if times==0 then
    return true
  elseif times>0 then
    movement.tryForward(times)
    return true
  end
end

-- Goes up or down a set amount of @times, depending on if @times is pos. or neg.
local function gotoY(times)
  if times==0 then
    return true
  elseif times>0 then
    movement.tryDown(times)
    return true
  elseif times<0 then
    local _times = (-1*times)
    movement.tryUp(_times)
    return true
  end
end

-- Turns form the current Angle towards @target Angle.
local function turnToAngle(target)
  if angle~=nil then
    if angle~=target then
      if angle>target then
        movement.turn((angle-target), 2)
        return true
      end
      if angle<target then
        movement.turn((target-angle), 1)
        return true
      end
    else
      return true
    end
  else
    return false
  end
end

-- Forces to set the current Angle.(Not recommended!)
function movement.setAngle(_angle)
  angle=_angle
end

return movement