Refactored velocity configuration in control algorithm. Made default PID constants the good ones. Corrected the velocity sign. There might be sign errors with the velocity PID constants.

2. Switched to level 5 IMU filter
3. logging setpoints
4. Logging for up to 5 minutes
5. Also accounting for time spent during logging when calculating LOOP_TIME
6. Logging during manual mode now, too.

PID nodes have a second parameter, called alpha that is (1 - N*T_s), where N is equivalent to Simulink N.
If alpha == 0, no filtering (default).
Also fixed issue where sampling time for autonomous was 0 for the first iteration.

Shuffled around #include dependencies to allow the control algorithm diagram to be generated without copying code out of control_algorithm.c