Team 581's FRC 2022 Rapid React robot code

Features

Zero, one, or two ball autonomous
Drivetrain
- Mecanum drivetrain controlled with a holonomic drive controller during autonomous
- Per-wheel closed-loop velocity control with feedforward and feedback via PID
Vision
- Two Limelight 2+s used for tracking cargo and aligning with the upper hub
- Uses on-the-fly trajectory generation for aligning with the hub's fenders during teleop
- Full robot localization using vision data fused with odometry data using an unscented Kalman filter
Arm which carries the intake & shooter (AKA "Swiffer Picker Upper")
- Closed-loop velocity control for intake & shooter flywheel
- Open-loop position control for moving arm
Misc
- Mass data logging to USB + streamed to driver station via Advantage Kit

Robot source code is stored within the src/main/java/frc/robot/ directory.

We also have a few files within the src/main/java/lib/ directory for generic vendor utilities (ex. a Limelight NetworkTables wrapper).

Directory	Description
`src/main/java/frc/robot/superstructure/`	The superstructure `Subsystem` that helps coordinate movement of the swiffer and arm together.
`src/main/java/frc/robot/superstructure/swiffer/`	The "Swiffer Picker Upper", our combined intake & shooter mechanism.
`src/main/java/frc/robot/superstructure/arm/`	The arm mechanism for the Swiffer Picker Upper's arm.
`src/main/java/frc/robot/vision_cargo/`	The vision system (Limelight 2+) used for targeting cargo on the floor.
`src/main/java/frc/robot/vision_upper/`	The vision system (Limelight 2+) used for targeting the upper hub.
`src/main/java/frc/robot/vision/`	Code reused in the vision subsystems.
`src/main/java/frc/robot/paths/`	Tools for autonomously following paths/trajectories.
`src/main/java/frc/robot/drive/`	The drivetrain.
`src/main/java/frc/robot/controller/`	Game controllers used by human drivers.
`src/main/java/frc/robot/misc/`	Miscellaneous classes that are used throughout the project and don't belong to any one group.

PathPlanner trajectories are stored within the src/main/deploy/pathplanner/ directory.

Assorted WPILib SysId files are stored in the top-level project directory and the .SysId/ directory.

File	Description
`config.json`	The main SysId configuration file.
`sys_id_data*.json`	Recorded SysId data files for analysis.