Reorganized constants and common functions, fixed TeleOp, and updated Autonomous