Fixed drive code and other systems. Cleaned up code structures. Added basic joystick functions and user input into global loop. Added systems to global loop. Code is now ready for the robot.