#print('diff ', math.degrees(diff), '\n')
return diff
while i < len(jsonInstruction) -1:
if len(jsonInstruction) - 1 - i is 70:
print('################## SLOW SPEED #################')
MAX_SPEED = 0.5
current_position = get.getPose()
#calc rotation
#robot_direction = calc.direction(current_position['Pose']['Orientation']['Y'], current_position['Pose']['Orientation']['X'])
vector = calc.bearing(current_position['Pose']['Orientation'])
robot_direction = calc.direction(vector['Y'], vector['X'])
vectorX = current_instruction['Pose']['Position']['X'] - current_position['Pose']['Position']['X']
vectorY = current_instruction['Pose']['Position']['Y'] - current_position['Pose']['Position']['Y']
goal_direction = math.atan2(vectorY, vectorX)
#calc movement
robot_position = current_position['Pose']['Position']
deltaX = robot_position['X'] - goal_position['X']
deltaY = robot_position['Y'] - goal_position['Y']
distance = sqrt((deltaX)**2 + (deltaY)**2)
def getBearing():
"""Returns the XY Orientation as a bearing unit vector"""
return calc.bearing(getPose()["Pose"]["Orientation"])
