This Is just some Ideas to get our team started, some code is broken, other is incomplete.
Please be cautious using this code if you don't understand how to use cpp files with your ino (Arduino) files.
I've used pointer notation with the external objects, most of the objects have an ino file that gives examples of how to use it if you include the cpp and h files.
Happy to help if you want to contact me phil.legge@det.nsw.edu.au. and also feel free to improve any of these libraries or give me ideas to improve them.
Please keep in mind these small code examples and libraries were written to help students be able to engage with the open robocup competition, they are not full solutions nor are they produced with the most efficient methods but they were created to enable students to design solutions to the game.