Shoot While Move and Auton changes

Robocubs · Apr 12, 2024 · 3aa9264 · 3aa9264
1 parent f9054be
commit 3aa9264
Show file tree

Hide file tree

Showing 4 changed files with 46 additions and 27 deletions.
diff --git a/src/main/java/com/team1701/robot/commands/AutonomousCommands.java b/src/main/java/com/team1701/robot/commands/AutonomousCommands.java
@@ -120,6 +120,10 @@ private Command setNoteToSeek(AutoNote note) {
                 .withName("SetNoteToSeek");
     }
 
+    private Command clearNoteToSeek() {
+        return runOnce(() -> mAutoNoteSeeker.clear()).ignoringDisable(true).withName("ClearNoteToSeek");
+    }
+
     private Command timedDriveWithVelocity(ChassisSpeeds speeds, double seconds) {
         return DriveCommands.driveWithVelocity(() -> speeds, mDrive)
                 .withTimeout(seconds)
@@ -203,26 +207,39 @@ private Command driveBackPreWarmAndShoot(String pathName) {
                 runOnce(() -> mRobotState.setUseAutonFallback(false)));
     }
 
-    private Command driveToNextPiece(AutoNote nextNote) {
-        return new DriveAndSeekNote(
-                mDrive,
-                mRobotState,
-                new DriveToPose(
-                        mDrive,
-                        mRobotState,
-                        () -> new Pose2d(
-                                nextNote.pose().getTranslation(),
+    private class NextNote {
+        Pose2d notePose;
+
+        public NextNote() {
+            notePose = new Pose2d();
+        }
+    }
+
+    private Command driveToNextPiece(AutoNote note) {
+        NextNote nextNote = new NextNote();
+        return loggedSequence(
+                setNoteToSeek(note),
+                runOnce(() -> nextNote.notePose = new Pose2d(
+                                note.pose().getTranslation(),
                                 mRobotState
                                         .getPose2d()
                                         .getTranslation()
-                                        .minus(nextNote.pose().getTranslation())
-                                        .getAngle()),
-                        mRobotState::getPose2d,
-                        kAutoTrapezoidalKinematicLimits,
-                        true,
-                        true),
-                mAutoNoteSeeker::getDetectedNoteToSeek,
-                kAutoTrapezoidalKinematicLimits);
+                                        .minus(note.pose().getTranslation())
+                                        .getAngle())
+                        .transformBy(Constants.Robot.kIntakeToRobot)),
+                new DriveAndSeekNote(
+                        mDrive,
+                        mRobotState,
+                        new DriveToPose(
+                                mDrive,
+                                mRobotState,
+                                () -> nextNote.notePose,
+                                mRobotState::getPose2d,
+                                kAutoTrapezoidalKinematicLimits,
+                                true,
+                                true),
+                        mAutoNoteSeeker::getDetectedNoteToSeek,
+                        kAutoTrapezoidalKinematicLimits));
     }
 
     private Pose2d getFirstPose(String pathName) {
@@ -254,7 +271,7 @@ private Command efficientlyPreWarmShootAndDrive(String pathName, String returnPa
                                         idle(),
                                         runOnce(() -> mRobotState.setUseAutonFallback(true)),
                                         mRobotState::hasNote)))
-                .andThen(either(driveToNextPiece(nextNote).withTimeout(3), none(), mRobotState::getUseAutonFallback));
+                .andThen(either(driveToNextPiece(nextNote), none(), mRobotState::getUseAutonFallback));
     }
 
     private Command followChoreoPathAndSeekNote(String pathName) {
@@ -266,12 +283,13 @@ private Command followChoreoPathAndSeekNote(String pathName) {
         mPathBuilder.addPath(trajectory.getPoses());
 
         var eventMarkers = ChoreoEventMarker.loadFromFile(pathName);
-        return new DriveAndSeekNote(
+        return clearNoteToSeek()
+                .andThen(new DriveAndSeekNote(
                         mDrive,
                         mRobotState,
                         new DriveChoreoTrajectory(mDrive, mRobotState, trajectory, eventMarkers, false),
                         mAutoNoteSeeker::getDetectedNoteToSeek,
-                        kAutoTrapezoidalKinematicLimits)
+                        kAutoTrapezoidalKinematicLimits))
                 .deadlineWith(index(), idleShooter())
                 .finallyDo(mAutoNoteSeeker::clear)
                 .withName("FollowChoreoAndSeekNote");

diff --git a/src/main/java/com/team1701/robot/commands/ShootCommands.java b/src/main/java/com/team1701/robot/commands/ShootCommands.java
@@ -19,9 +19,10 @@
 public class ShootCommands {
     public static Command idleShooterCommand(Shooter shooter, RobotState robotState) {
         return Commands.runEnd(
-                () -> shooter.setSetpoint(ShooterUtil.calculateIdleSetpoint(robotState)),
-                shooter::stopRotation,
-                shooter);
+                        () -> shooter.setSetpoint(ShooterUtil.calculateIdleSetpoint(robotState)),
+                        shooter::stopRotation,
+                        shooter)
+                .withName("IdleShooterCommand");
     }
 
     public static Command stop(Shooter shooter) {

diff --git a/src/main/java/com/team1701/robot/simulation/NoteSimulator.java b/src/main/java/com/team1701/robot/simulation/NoteSimulator.java
@@ -55,8 +55,8 @@ public class NoteSimulator extends SubsystemBase {
                     AutoNote.A.redPose(),
                     AutoNote.B.redPose(),
                     AutoNote.C.redPose(),
-                    AutoNote.M1.pose(),
-                    AutoNote.M2.pose(),
+                    // AutoNote.M1.pose(),
+                    // AutoNote.M2.pose(),
                     AutoNote.M3.pose(),
                     AutoNote.M4.pose(),
                     AutoNote.M5.pose())

diff --git a/src/main/java/com/team1701/robot/states/RobotState.java b/src/main/java/com/team1701/robot/states/RobotState.java
@@ -279,8 +279,8 @@ public Rotation2d getToleranceSpeakerHeading(Translation2d translation) {
                 .minus(translation)
                 .getAngle();
 
-        var toleranceRadians = Math.abs(
-                MathUtil.angleModulus(heading.getRadians() - getSpeakerHeading().getRadians()));
+        var toleranceRadians = Math.abs(MathUtil.angleModulus(heading.getRadians()
+                - FieldUtil.getHeadingToSpeaker(translation).getRadians()));
 
         return Rotation2d.fromRadians(Math.max(0.017, toleranceRadians / 2));
     }