from Robot import *
from Path import *
from math import *
import json

robot = Robot()

# move the robot
robot.setMotion(0.5, 0.5)

for i in range(10):
    time.sleep(1)


# load a path file
#p = Path("Path-around-table.json")
#path = p.getPath()

#Räkna ut robotens vinkel
robotAngle = robot.getHeading(pose)

#koordinats position, lista av dictionaries
coordinate_x = path[0]['X']
coordinate_y = path[0]['Y']

#robotens position, returnerar två värden
robot_x = robot.getPosition[0]
robot_y = robot.getPosition[1]


dx = robot_x - coordinate_x
dy = robot_y - coordinate_y
angle = atan2(dy, dx)

#räkna ut angle - getHeading (vinkel)
angle-robotAngle


for i in range(10):
    time.sleep(0.5)
    print("pos, heading")
    print(robot.getPosition())
    print(robot.getHeading())

echoes = robot.getLaser()
print(echoes)

robot.setMotion(0.2, 0.2)