Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

April tag align #1

Closed
wants to merge 13 commits into from
Prev Previous commit
Feat: Field allignment positions added
  • Loading branch information
ProgrammingSR committed Jan 20, 2024
commit acacb7f1b2959c30bab21628aac7db310c0e5f90
32 changes: 18 additions & 14 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
@@ -100,20 +100,24 @@ public final static class TagAlignConstants {
public final static double kTagPIDkDomega = 0;

public static final Pose2d[] kTargetPoses = new Pose2d[] {
new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), //Amp
new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), //SourceLeft
new Pose2d(new Translation2d(0, 0), new Rotation2d(0)), //etc.
new Pose2d(new Translation2d(0, 0), new Rotation2d(0)),
new Pose2d(new Translation2d(0, 0), new Rotation2d(0)),
new Pose2d(new Translation2d(0, 0), new Rotation2d(0)),
new Pose2d(new Translation2d(0, 0), new Rotation2d(0)),
new Pose2d(new Translation2d(0, 0), new Rotation2d(0)),
new Pose2d(new Translation2d(0, 0), new Rotation2d(0)),
new Pose2d(new Translation2d(0, 0), new Rotation2d(0)),
new Pose2d(new Translation2d(0, 0), new Rotation2d(0)),
new Pose2d(new Translation2d(0, 0), new Rotation2d(0)),
new Pose2d(new Translation2d(0, 0), new Rotation2d(0)),
new Pose2d(new Translation2d(0, 0), new Rotation2d(0)) //StageCenterRight
new Pose2d(new Translation2d(585.75, 313), new Rotation2d(Math.toRadians(270))), //Amp //Change y based on length of robot using 28in long, Red side

//Asumed that these are positions for a single source, so that you have 3 positions for the source on the left
//Left , center, right, all on same obj //TODO: Make sure these work with robot demensions/bumpers
new Pose2d(new Translation2d(590, 9), new Rotation2d(Math.toRadians(120))), //SourceLeft
new Pose2d(new Translation2d(631.125, 20.625), new Rotation2d(Math.toRadians(120))), //etc.
new Pose2d(new Translation2d(637, 34), new Rotation2d(Math.toRadians(120))),

new Pose2d(new Translation2d(598.125, 222.75), new Rotation2d(0)), //TODO: Check this (Red speaker)
new Pose2d(new Translation2d(464.5, 117.315), new Rotation2d(Math.toRadians(300))),
new Pose2d(new Translation2d(421.115, 161.62), new Rotation2d(Math.toRadians(180))),
new Pose2d(new Translation2d(459.75, 197.725), new Rotation2d(Math.toRadians(60))),
new Pose2d(new Translation2d(186.855, 195.662), new Rotation2d(Math.toRadians(120))),
new Pose2d(new Translation2d(182.245, 161.62), new Rotation2d(0)),
new Pose2d(new Translation2d(193.875, 123.75), new Rotation2d(Math.toRadians(240))),
new Pose2d(new Translation2d(78.375, 165), new Rotation2d(0)),
new Pose2d(new Translation2d(330, 165), new Rotation2d(0)), //Switch angle by 1pi if you want opposite rotation
new Pose2d(new Translation2d(585.75, 165), new Rotation2d(0)) //StageCenterRight
}; // Poses are in the same order as the enumerator.
// TODO: populate poses