from Robot import Robot
from time import sleep
from Path import Path
from Calibrate import Calibrate
from AngleConverter import AngleConverter
from PathHandler import PathHandler
from Navigator import Navigator
from Goal import Goal
# load a path file
#p = Path("Path-around-table.json")
p = Path("Path-around-table-and-back.json")
#p = Path("Path-from-bed.json")
#p = Path("Path-to-bed.json")
path = p.getPath()
print("Path length = " + str(len(path)))
print("First point = " + str(path[0]['X']) + ", " + str(path[0]['Y']))
# make a robot to move around
robot = Robot()
converter = AngleConverter()
pathHandler = PathHandler()
navigator = Navigator()
goal = Goal()
#### Intialize variables
position = robot.getPosition()
speed = 0.25
heading = 0
nextPoint = 0
