Add test file with main method
Make DriveTo move car to next node, add makefile