AlignWithReefButton

[ethanharjabrata]

[ethanharjabrata]

This command must take the angle relative to april tag on reef and distance to april tag, then align the robot's orientation and postion such that it will be in the perfect position to deposit the coral

[ethanharjabrata]

What will probably need to be done is:

1. Using PID and the value generated from the Camera subsystem, align robot until it is parallel to April Tag
2. Using PID and the value generated from the Camera subsystem, get robot position a set distance from coral. Distance required should be left in the Constants file. Do note that the corals are left and right of the april tag, so probably seperate buttons are needed to align with left reef and align with right reef

[ethanharjabrata]

https://firstfrc.blob.core.windows.net/frc2025/Manual/Sections/2025GameManual-05ARENA.pdf

Coral pipes are 33cm apart, so 16.5 left and 16.5 right of center of april tag

[ethanharjabrata]

Quick note that if you pass a positive value for the "ySpeed" parameter of the "drive()" method of the "DriveSubsystem", it causes the robot to drive to the left, not the right

[ethanharjabrata]

To do list:

1. Make sure camera odometry values are still working. Probably do this by driving robot manually and printing the odometry values on SmartDashboard.
2. Make sure that April Tag Align has a proper cancel button. Use the same logic I used in the "Coral Station Align" command.
3. Tune the April Tag Align PID values