Make AutomatedTeleop use drivetrain auto alignment

team401 · Apr 28, 2024 · 1cd0e2e · 1cd0e2e
1 parent 66da672
commit 1cd0e2e
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/commands/AutomatedTeleop.java b/src/main/java/frc/robot/commands/AutomatedTeleop.java
@@ -6,6 +6,8 @@
 import frc.robot.Constants.AutomatedTeleopConstants;
 import frc.robot.Constants.FieldConstants;
 import frc.robot.subsystems.drive.CommandSwerveDrivetrain;
+import frc.robot.subsystems.drive.CommandSwerveDrivetrain.AlignState;
+import frc.robot.subsystems.drive.CommandSwerveDrivetrain.AlignTarget;
 import frc.robot.subsystems.intake.IntakeSubsystem;
 import frc.robot.subsystems.intake.IntakeSubsystem.IntakeAction;
 import frc.robot.subsystems.scoring.ScoringSubsystem;
@@ -127,9 +129,14 @@ public void execute() {
                 }
 
                 drivetrain.driveToSpeaker();
+
+                // Begin aligning to the speaker so robot is already aimed when it's ready to shoot
+                drivetrain.setAlignTarget(AlignTarget.SPEAKER);
+                drivetrain.setAlignState(AlignState.ALIGNING);
+
                 scoringSubsystem.setAction(ScoringAction.AIM);
 
-                // Once we are in range, shoot
+                // Once robot is in range, shoot
                 if (scoringSubsystem.findDistanceToGoal()
                         < AutomatedTeleopConstants.shootRangeMeters) {
                     state = State.SHOOT_NOTE;
@@ -140,7 +147,10 @@ public void execute() {
                     state = State.DRIVE_TO_SOURCE;
                 }
 
+                drivetrain.setAlignTarget(AlignTarget.SPEAKER);
+                drivetrain.setAlignState(AlignState.ALIGNING);
                 drivetrain.driveToSpeaker();
+
                 scoringSubsystem.setAction(ScoringAction.SHOOT);
 
                 break;

diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java
@@ -556,6 +556,7 @@ public void driveToPose(Pose2d targetPose) {
 
     public void driveToPath(String pathName) {
         if (pathName == lastCommandedPath) {
+            // TODO: Determine if we need/want this check
             // return;
         } else {
             lastCommandedPath = pathName;
@@ -574,6 +575,7 @@ public void driveToPath(String pathName) {
                 (target) -> {
                     Logger.recordOutput("targetPose", target);
                 });
+
         // Logging callback for the active path, this is sent as a list of poses
         PathPlannerLogging.setLogActivePathCallback(
                 (poses) -> {