Kiwi Drive and Integrating PID Loops

[jgrindle]

The Cg is about 8.45 inches from the back center wheel, while 20.5 inches from the front side wheels. When we adds weight, we will try to make the CG at the point of intersection, to minimize the change wed need to do to theta. Thus making the scaling(if any at that point) very small numbers.

If anyone wants to help contribute to the code, I have a Github Repo that you can look at, modify, and point out any noticeable bugs. I'd suggest avoiding intake, and elevator at the moment, as they are not ready.