Fixed bug with angles.
Building a graph. Start.

When reaching an intersection, find the closest node.
If the node is less than 7.5 cm away, assume this is
a node we have seen before.
Otherwise create a new node in the graph.