Added more stuff for the PID contollers and put in some values for the low pass filters (They may be too high now that I'm thinking of it)