Shoot While Move, Passing, and Auton Tweaks

src/main/java/com/team1701/robot/Constants.java

@@ -433,10 +433,10 @@ public static final class Shooter {
                 new LoggedTunableNumber("Shooter/Rotation/TunableAngleRadians", 1.0);
 
         public static final LoggedTunableNumber kLowerAmpSpeed =
-                new LoggedTunableNumber("Shooter/Roller/Amp/LowerRollerSpeed", 70);
+                new LoggedTunableNumber("Shooter/Roller/Amp/LowerRollerSpeed", 75);
 
         public static final LoggedTunableNumber kUpperAmpSpeed =
-                new LoggedTunableNumber("Shooter/Roller/Amp/UpperRollerSpeed", 70);
+                new LoggedTunableNumber("Shooter/Roller/Amp/UpperRollerSpeed", 75);
 
         public static final LoggedTunableNumber kRotationKp = new LoggedTunableNumber("Shooter/Motor/Rotation/Kp");
         public static final LoggedTunableNumber kRotationKd = new LoggedTunableNumber("Shooter/Motor/Rotation/Kd");
@@ -495,10 +495,11 @@ public static final class Shooter {
         };
 
         public static final double[][] kShooterForwardVelocityToHeadingOffset = {
-            {66, 13},
-            {73, 9},
-            {89, 2},
-            {100, 1}
+            {315 * Math.cos(.975), 9},
+            {370 * Math.cos(.689), 6},
+            {437.2 * Math.cos(.483), 4},
+            {493 * Math.cos(.4), 2},
+            {560 * Math.cos(.306), 0}
         };
 
         public static final double[][] kPassingDistanceToAngleValues = {

src/main/java/com/team1701/robot/RobotContainer.java

@@ -387,7 +387,7 @@ private void setupControllerBindings() {
         mDriverController
                 .leftBumper()
                 .and(() -> mRobotState.getScoringMode().equals(ScoringMode.SPEAKER))
-                .whileTrue(ShootCommands.passANote(
+                .whileTrue(DriveCommands.passANote(
                         mDrive,
                         mShooter,
                         mIndexer,

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)
@@ -141,7 +145,7 @@ private Command driveToPoseAndPreWarm(Pose2d pose) {
             return stopRoutine();
         }
 
-        var setpoint = ShooterUtil.calculateSetpoint(FieldUtil.getDistanceToSpeaker(pose.getTranslation()));
+        var setpoint = ShooterUtil.calculateShooterSetpoint(FieldUtil.getDistanceToSpeaker(pose.getTranslation()));
         mPathBuilder.addPose(pose);
         return DriveCommands.driveToPose(
                         mDrive,
@@ -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) {
@@ -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");
@@ -289,7 +307,8 @@ private Command followChoreoPathAndPreWarm(String pathName, boolean resetPose, b
 
         mPathBuilder.addPath(trajectory.getPoses());
 
-        var shooterSetpoint = ShooterUtil.calculateSetpoint(FieldUtil.getDistanceToSpeaker(trajectory.getFinalPose()));
+        var shooterSetpoint =
+                ShooterUtil.calculateShooterSetpoint(FieldUtil.getDistanceToSpeaker(trajectory.getFinalPose()));
         var eventMarkers = ChoreoEventMarker.loadFromFile(pathName);
         return new DriveChoreoTrajectory(
                         mDrive, mRobotState, trajectory, eventMarkers, shooterSetpoint::applyReleaseAngle, resetPose)
@@ -305,7 +324,8 @@ private Command followChoreoPathAndShoot(String pathName, boolean resetPose, dou
 
         mPathBuilder.addPath(trajectory.getPoses());
 
-        var shooterSetpoint = ShooterUtil.calculateSetpoint(FieldUtil.getDistanceToSpeaker(trajectory.getFinalPose()));
+        var shooterSetpoint =
+                ShooterUtil.calculateShooterSetpoint(FieldUtil.getDistanceToSpeaker(trajectory.getFinalPose()));
         var eventMarkers = ChoreoEventMarker.loadFromFile(pathName);
         return new DriveChoreoTrajectory(
                         mDrive, mRobotState, trajectory, eventMarkers, shooterSetpoint::applyReleaseAngle, resetPose)

src/main/java/com/team1701/robot/commands/DriveCommands.java

@@ -229,4 +229,26 @@ public static Command driveWithVelocity(Supplier<ChassisSpeeds> velocity, Drive
                 .finallyDo(() -> drive.stop())
                 .withName("DriveWithVelocity");
     }
+
+    public static Command passANote(
+            Drive drive,
+            Shooter shooter,
+            Indexer indexer,
+            RobotState robotState,
+            DoubleSupplier throttleSupplier,
+            DoubleSupplier strafeSupplier) {
+        return new PassANote(
+                        drive,
+                        shooter,
+                        indexer,
+                        robotState,
+                        () -> calculateDriveWithJoysticksVelocities(
+                                        throttleSupplier.getAsDouble(),
+                                        strafeSupplier.getAsDouble(),
+                                        drive.getFieldRelativeHeading(),
+                                        Constants.Drive.kFastTrapezoidalKinematicLimits.maxDriveVelocity())
+                                .rotateBy(robotState.getHeading()),
+                        () -> new Rotation2d(.03))
+                .withName("PassANote");
+    }
 }

src/main/java/com/team1701/robot/commands/PassANote.java

@@ -1,16 +1,30 @@
 package com.team1701.robot.commands;
 
+import java.util.function.Supplier;
+
+import com.team1701.lib.swerve.SwerveSetpointGenerator.KinematicLimits;
 import com.team1701.lib.util.GeometryUtil;
+import com.team1701.lib.util.TimeLockedBoolean;
+import com.team1701.lib.util.Util;
 import com.team1701.lib.util.tuning.LoggedTunableBoolean;
 import com.team1701.lib.util.tuning.LoggedTunableNumber;
+import com.team1701.lib.util.tuning.LoggedTunableValue;
 import com.team1701.robot.Constants;
 import com.team1701.robot.states.RobotState;
 import com.team1701.robot.states.ShootingState;
+import com.team1701.robot.subsystems.drive.Drive;
 import com.team1701.robot.subsystems.indexer.Indexer;
 import com.team1701.robot.subsystems.shooter.Shooter;
 import com.team1701.robot.subsystems.shooter.Shooter.ShooterSetpoint;
+import com.team1701.robot.util.FieldUtil;
 import com.team1701.robot.util.ShooterUtil;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import org.littletonrobotics.junction.Logger;
 
@@ -21,48 +35,143 @@ public class PassANote extends Command {
             new LoggedTunableBoolean(kLoggingPrefix + "TuningEnabled", false);
 
     private static final LoggedTunableNumber kAngleToleranceRadians =
-            new LoggedTunableNumber(kLoggingPrefix + "AngleToleranceRadians", 0.01);
+            new LoggedTunableNumber(kLoggingPrefix + "AngleToleranceRadians", 0.02);
+
+    private static final TrapezoidProfile.State kZeroState = new TrapezoidProfile.State(0.0, 0.0);
+    private static final KinematicLimits kKinematicLimits = Constants.Drive.kFastTrapezoidalKinematicLimits;
+
+    private static final LoggedTunableNumber kMaxAngularVelocity = new LoggedTunableNumber(
+            kLoggingPrefix + "MaxAngularVelocity",
+            Constants.Drive.kFastTrapezoidalKinematicLimits.maxDriveVelocity() / Constants.Drive.kModuleRadius);
+    private static final LoggedTunableNumber kMaxAngularAcceleration =
+            new LoggedTunableNumber(kLoggingPrefix + "MaxAngularAcceleration", kMaxAngularVelocity.get() / 2.0);
+
+    private Supplier<Rotation2d> mHeadingTolerance;
 
+    private final Drive mDrive;
     private final Shooter mShooter;
     private final Indexer mIndexer;
     private final RobotState mRobotState;
+    private final Supplier<Translation2d> mFieldRelativeSpeeds;
+    private final PIDController mRotationController;
 
-    private boolean mShooting;
+    private TrapezoidProfile mRotationProfile;
+    private TrapezoidProfile.State mRotationState = kZeroState;
 
-    public PassANote(Shooter shooter, Indexer indexer, RobotState robotState) {
+    private boolean mShooting;
+    private TimeLockedBoolean mLockedReadyToShoot;
+
+    public PassANote(
+            Drive drive,
+            Shooter shooter,
+            Indexer indexer,
+            RobotState robotState,
+            Supplier<Translation2d> fieldRelativeSpeeds,
+            Supplier<Rotation2d> headingTolerance) {
+        mDrive = drive;
         mShooter = shooter;
         mIndexer = indexer;
         mRobotState = robotState;
+        mFieldRelativeSpeeds = fieldRelativeSpeeds;
+        mRotationController = new PIDController(6.0, 0, 0, Constants.kLoopPeriodSeconds);
+        mRotationController.enableContinuousInput(-Math.PI, Math.PI);
+        mRotationProfile = new TrapezoidProfile(
+                new TrapezoidProfile.Constraints(kMaxAngularVelocity.get(), kMaxAngularAcceleration.get()));
+        mHeadingTolerance = headingTolerance;
+
+        mLockedReadyToShoot = new TimeLockedBoolean(.1, Timer.getFPGATimestamp());
 
-        addRequirements(shooter, indexer);
+        addRequirements(mDrive, shooter, indexer);
     }
 
     @Override
     public void initialize() {
         mShooting = false;
+        mDrive.setKinematicLimits(Constants.Drive.kFastKinematicLimits);
+
+        mRotationController.reset();
+        mRotationController.enableContinuousInput(-Math.PI, Math.PI);
+
+        var fieldRelativeChassisSpeeds = mDrive.getFieldRelativeVelocity();
+        var headingError = mRobotState.getHeading().minus(mRobotState.getPassingHeading());
+        mRotationState = new TrapezoidProfile.State(
+                MathUtil.angleModulus(headingError.getRadians()), fieldRelativeChassisSpeeds.omegaRadiansPerSecond);
+
+        mLockedReadyToShoot.update(false, Timer.getFPGATimestamp());
+
+        LoggedTunableValue.ifChanged(
+                hashCode(),
+                () -> {
+                    mRotationProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(
+                            Math.min(
+                                    kMaxAngularVelocity.get(),
+                                    kKinematicLimits.maxDriveVelocity() / Constants.Drive.kModuleRadius),
+                            Math.min(
+                                    kMaxAngularAcceleration.get(),
+                                    kKinematicLimits.maxDriveAcceleration() / Constants.Drive.kModuleRadius)));
+                    mRotationController.setPID(6, 0, 0);
+                },
+                kMaxAngularVelocity,
+                kMaxAngularAcceleration);
     }
 
     @Override
     public void execute() {
-        var setpoint = mTuningEnabled.get()
+        var currentPose = mRobotState.getPose2d();
+        var fieldRelativeSpeeds = mFieldRelativeSpeeds.get();
+
+        var droppedVelocity = ShooterUtil.calculatePassingSpeed(mRobotState.getDistanceToSpeaker())
+                * Constants.Shooter.kRollerSpeedToNoteSpeed;
+        var robotVelocityTowardsTarget = fieldRelativeSpeeds
+                .rotateBy(mRobotState.getPassingHeading().unaryMinus())
+                .getX();
+
+        var timeInAir = mRobotState.getPassingDistance() / (robotVelocityTowardsTarget + droppedVelocity);
+
+        var endTranslation = new Translation2d(
+                currentPose.getX() + fieldRelativeSpeeds.getX() * timeInAir,
+                currentPose.getY() + fieldRelativeSpeeds.getY() * timeInAir);
+
+        var shooterSetpoint = mTuningEnabled.get()
                 ? new ShooterSetpoint(
                         Constants.Shooter.kTunableShooterSpeedRadiansPerSecond.get(),
                         Rotation2d.fromRadians(Constants.Shooter.kTunableShooterAngleRadians.get()))
-                : ShooterUtil.calculatePassingSetpoint(mRobotState);
+                : ShooterUtil.calculatePassingSetpoint(FieldUtil.getDistanceToPassTarget(endTranslation));
+
+        mShooter.setSetpoint(shooterSetpoint);
+
+        var targetHeading = mRobotState.getPassingHeading().minus(shooterSetpoint.releaseAngle());
+        var headingError = mRobotState.getHeading().minus(targetHeading);
+
+        Rotation2d headingSetpoint;
+        double rotationalVelocity;
+        if (GeometryUtil.isNear(GeometryUtil.kRotationIdentity, headingError, mHeadingTolerance.get())
+                && Util.epsilonEquals(fieldRelativeSpeeds.getX(), 0)
+                && Util.epsilonEquals(fieldRelativeSpeeds.getY(), 0)) {
+            rotationalVelocity = 0;
+            mRotationState = kZeroState;
+            headingSetpoint = targetHeading;
+        } else {
+            var rotationPidOutput = mRotationController.calculate(headingError.getRadians(), 0);
+            mRotationState = mRotationProfile.calculate(Constants.kLoopPeriodSeconds, mRotationState, kZeroState);
+            rotationalVelocity = mRotationState.velocity + rotationPidOutput;
+            headingSetpoint = Rotation2d.fromRadians(targetHeading.getRadians() + mRotationState.position);
+        }
 
-        mShooter.setSetpoint(setpoint);
+        mDrive.setVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(
+                fieldRelativeSpeeds.getX(), fieldRelativeSpeeds.getY(), rotationalVelocity, currentPose.getRotation()));
 
-        var atSpeed = setpoint.speeds().allMatch(mShooter.getRollerSpeedsRadiansPerSecond(), 50.0);
+        var atSpeed = shooterSetpoint.speeds().allMatch(mShooter.getRollerSpeedsRadiansPerSecond(), 50.0);
 
         var atAngle = GeometryUtil.isNear(
-                mShooter.getAngle(), setpoint.angle(), Rotation2d.fromRadians(kAngleToleranceRadians.get()));
+                mShooter.getAngle(), shooterSetpoint.angle(), Rotation2d.fromRadians(kAngleToleranceRadians.get()));
 
         var atHeading = GeometryUtil.isNear(
-                mRobotState.getPassingHeading(), mRobotState.getHeading(), Constants.Shooter.kPassingHeadingTolerance);
+                targetHeading, mRobotState.getHeading(), Constants.Shooter.kPassingHeadingTolerance);
 
         var atPose = mRobotState.getPassingDistance() > 6 && !mRobotState.inOpponentWing();
 
-        if (atSpeed && atAngle && atHeading && atPose) {
+        if (mLockedReadyToShoot.update(atSpeed && atAngle && atHeading && atPose, Timer.getFPGATimestamp())) {
             mIndexer.setForwardShoot();
             mShooting = true;
         }
@@ -75,13 +184,18 @@ public void execute() {
             }
         }
 
-        mRobotState.setShootingState(new ShootingState(setpoint, true, atAngle, atSpeed, atHeading, mShooting));
+        mRobotState.setShootingState(new ShootingState(shooterSetpoint, true, atAngle, atSpeed, atHeading, mShooting));
         Logger.recordOutput(kLoggingPrefix + "AtPose", atPose);
+        Logger.recordOutput(kLoggingPrefix + "HeadingSetpoint", headingSetpoint);
     }
 
     @Override
     public void end(boolean interrupted) {
+        mShooting = false;
+
+        mRobotState.setShootingState(ShootingState.kDefault);
         mShooter.stop();
         mIndexer.stop();
+        mDrive.stop();
     }
 }

src/main/java/com/team1701/robot/commands/Shoot.java

@@ -87,7 +87,7 @@ public void execute() {
                 currentPose.getY() + fieldRelativeSpeeds.vyMetersPerSecond * timeInAir);
 
         var setpoint = mRobotState.isSpeakerMode()
-                ? ShooterUtil.calculateSetpoint(FieldUtil.getDistanceToSpeaker(endTranslation))
+                ? ShooterUtil.calculateShooterSetpoint(FieldUtil.getDistanceToSpeaker(endTranslation))
                 : ShooterUtil.calculateStationarySetpoint(mRobotState);
 
         mShooter.setSetpoint(setpoint);