Robot-2025-Code

4572 Robot Code for the 2025 FIRST Robotics Competition

TODO

1. Autalign fix, use two different cameras or limelights mounted on swerve
2. Add 2 camera aproach.
3. Get Climb working
4. Fix L3 problem with elevator.
5. Make autoalign without pathplanner OTF
6. Code climb
7. Fix L4
8. Alter algae intake
9. Refine code