Make auto changes

src/main/java/frc/robot/Autos.java

@@ -4,6 +4,7 @@
 import com.choreo.lib.ChoreoControlFunction;
 import com.choreo.lib.ChoreoTrajectory;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.Timer;
@@ -18,8 +19,9 @@
 import frc.robot.intake.Intake;
 import frc.robot.lights.Lights;
 import frc.robot.shooter.Shooter;
+import frc.robot.vision.GamePieceDetection;
 import java.util.Set;
-import java.util.function.BooleanSupplier;
+import org.littletonrobotics.junction.Logger;
 
 public class Autos {
 
@@ -31,24 +33,27 @@ public class Autos {
   private final Intake m_intake;
   private final Arm m_arm;
   private final Lights m_lights;
+  private final GamePieceDetection m_gamePieceDetection;
 
   private final PIDController m_translationController = new PIDController(3.0, 0.0, 0.0);
-  private final PIDController m_rotationController = new PIDController(3.0, 0.0, 0.0);
-  private boolean shootingWhileMoving = false;
+  private final PIDController m_yControllerGamePieceError = new PIDController(0.1, 0.0, 0.0);
+  private final PIDController m_rotationController = new PIDController(5.0, 0.0, 0.0);
 
   public Autos(
       Drivetrain drivetrain,
       Shooter shooter,
       Feeder feeder,
       Intake intake,
       Arm arm,
-      Lights lights) {
+      Lights lights,
+      GamePieceDetection gamePieceDetection) {
     m_drivetrain = drivetrain;
     m_shooter = shooter;
     m_feeder = feeder;
     m_intake = intake;
     m_arm = arm;
     m_lights = lights;
+    m_gamePieceDetection = gamePieceDetection;
   }
 
   public Command subShot() {
@@ -137,27 +142,12 @@ public Command ampSide() {
         .withName("Amp Side 1.5 Piece");
   }
 
-  public Command centerlineDisrupt() {
-    return Commands.sequence(
-            resetPose("CenterlineDisrupt"),
-            toggleShootingWhileMoving(),
-            Commands.deadline(
-                getPathFollowingCommand(
-                    "CenterlineDisrupt.1",
-                    shootWhileMovingControlFunction(() -> shootingWhileMoving)),
-                aimAndShoot().andThen(toggleShootingWhileMoving())))
-        .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)
-        .withName("Centerline Disrupt");
-  }
-
-  private Command toggleShootingWhileMoving() {
-    return Commands.runOnce(() -> shootingWhileMoving = !shootingWhileMoving);
-  }
-
   private Command getPathFollowingCommand(
       String trajectoryName, ChoreoControlFunction controlFunction) {
+    final ChoreoTrajectory trajectory = Choreo.getTrajectory(trajectoryName);
+    Logger.recordOutput("Drivetrain/Trajectory", trajectory.getPoses());
     return Choreo.choreoSwerveCommand(
-            Choreo.getTrajectory(trajectoryName),
+            trajectory,
             PoseEstimation.getInstance()::getPose,
             controlFunction,
             m_drivetrain::runVelocity,
@@ -169,28 +159,32 @@ private Command getPathFollowingCommand(
   private Command getPathFollowingCommand(String trajectoryName) {
     return getPathFollowingCommand(
         trajectoryName,
-        Choreo.choreoSwerveController(
+        choreoSwerveController(
             m_translationController, m_translationController, m_rotationController));
   }
 
-  private ChoreoControlFunction shootWhileMovingControlFunction(BooleanSupplier shootingOnTheMove) {
+  public ChoreoControlFunction choreoSwerveController(
+      PIDController xController, PIDController yController, PIDController rotationController) {
+    rotationController.enableContinuousInput(-Math.PI, Math.PI);
     return (pose, referenceState) -> {
+      Logger.recordOutput(
+          "Drivetrain/TrajectorySetpoint",
+          new Pose2d(referenceState.x, referenceState.y, new Rotation2d(referenceState.heading)));
       double xFF = referenceState.velocityX;
       double yFF = referenceState.velocityY;
       double rotationFF = referenceState.angularVelocity;
 
-      double xFeedback = m_translationController.calculate(pose.getX(), referenceState.x);
-      double yFeedback = m_translationController.calculate(pose.getY(), referenceState.y);
+      double xFeedback = xController.calculate(pose.getX(), referenceState.x);
+      double yFeedback =
+          m_gamePieceDetection.hasValidTarget.getAsBoolean()
+              ? m_yControllerGamePieceError.calculate(
+                  m_gamePieceDetection.horizontalErrorDeg.getAsDouble(), 0.0)
+              : yController.calculate(pose.getY(), referenceState.y);
       double rotationFeedback =
-          m_rotationController.calculate(pose.getRotation().getRadians(), referenceState.heading);
-      double omegaRadiansPerSecond =
-          shootingOnTheMove.getAsBoolean() ? 0.0 : rotationFF + rotationFeedback;
-      Rotation2d robotRotation =
-          shootingOnTheMove.getAsBoolean()
-              ? PoseEstimation.getInstance().getAimingParameters().driveHeading()
-              : pose.getRotation();
+          rotationController.calculate(pose.getRotation().getRadians(), referenceState.heading);
+
       return ChassisSpeeds.fromFieldRelativeSpeeds(
-          xFF + xFeedback, yFF + yFeedback, omegaRadiansPerSecond, robotRotation);
+          xFF + xFeedback, yFF + yFeedback, rotationFF + rotationFeedback, pose.getRotation());
     };
   }
 
@@ -214,18 +208,7 @@ private Command aim() {
         .alongWith(
             Commands.defer(
                 () -> {
-                  if (shootingWhileMoving) {
-                    return Commands.startEnd(
-                        () ->
-                            m_drivetrain.setHeadingGoal(
-                                () ->
-                                    PoseEstimation.getInstance()
-                                        .getAimingParameters()
-                                        .driveHeading()),
-                        m_drivetrain::clearHeadingGoal);
-                  } else {
-                    return m_drivetrain.blankDrive();
-                  }
+                  return m_drivetrain.blankDrive();
                 },
                 Set.of()))
         .withName("Aim");

src/main/java/frc/robot/Robot.java

@@ -6,6 +6,7 @@
 import edu.wpi.first.net.PortForwarder;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -95,7 +96,7 @@ public Robot() {
                 : Subsystems.createTalonFXClimber();
         m_shooter = Subsystems.createTalonFXShooter();
         m_feeder = Subsystems.createTalonFXFeeder();
-        m_gamePieceDetection = Subsystems.createBlankGamePieceDetection();
+        m_gamePieceDetection = Subsystems.createLimelightGamePieceDetection();
         break;
       case kSim:
         m_drivetrain = Subsystems.createSimDrivetrain();
@@ -122,7 +123,9 @@ public Robot() {
     NoteVisualizer.setWristPoseSupplier(m_arm.wristPoseSupplier);
     NoteVisualizer.resetNotes();
     NoteVisualizer.showStagedNotes();
-    final Autos autos = new Autos(m_drivetrain, m_shooter, m_feeder, m_intake, m_arm, m_lights);
+    final Autos autos =
+        new Autos(
+            m_drivetrain, m_shooter, m_feeder, m_intake, m_arm, m_lights, m_gamePieceDetection);
     NamedCommands.registerCommand("intake", m_intake.intake());
     NamedCommands.registerCommand("intakeOff", m_intake.idle());
     NamedCommands.registerCommand("enableShooter", new ScheduleCommand(m_shooter.runShooter()));
@@ -208,6 +211,10 @@ public Robot() {
         .or(m_feeder.hasNote)
         .onTrue(Commands.runOnce(() -> NoteVisualizer.setHasNote(true)));
     m_feeder.hasNote.onTrue(m_lights.noteBlink());
+    m_gamePieceDetection
+        .hasValidTarget
+        .and(() -> DriverStation.isAutonomous())
+        .onTrue(m_lights.startBlink(Color.kBlue));
 
     m_driverController
         .rightTrigger()

src/main/java/frc/robot/Superstructure.java

@@ -75,7 +75,7 @@ public static Command aimAtStash(Drivetrain drivetrain, Shooter shooter, Arm arm
                                 .getAngle()
                                 .rotateBy(new Rotation2d(Math.PI))),
                 drivetrain::clearHeadingGoal),
-            shooter.stashShot(),
+            shooter.feederShot(),
             arm.goToSetpoint(ArmSetpoints.kStash),
             lights.showReadyToShootStatus(
                 drivetrain.atHeadingGoal.and(drivetrain.atHeadingGoal).and(arm.atGoal)))

src/main/java/frc/robot/arm/Arm.java

@@ -308,7 +308,7 @@ public enum ArmSetpoints {
     kAmp(1.49 + 0.0873 + Units.degreesToRadians(3.0), Units.degreesToRadians(228), 0.0, 0.0),
     kClimb(1.633, -2.371, 0.0, 0.0),
     kTrap(Units.degreesToRadians(53.0), Units.degreesToRadians(80.0), 0.0, 0.0),
-    kStash(Units.degreesToRadians(10.0), Units.degreesToRadians(170.0), 0.0, 0.0);
+    kStash(Units.degreesToRadians(-31), 2.083, 0.0, 0.0);
 
     public final double elbowAngle;
     public final double wristAngle;

src/main/java/frc/robot/drivetrain/Module.java

@@ -15,7 +15,7 @@ public class Module {
       Constants.kIsViper ? (50.0 / 13.0) : (50.0 / 14.0);
   public static final double kDriveRatio = kDriveRatioFirstStage * (16.0 / 28.0) * (45.0 / 15.0);
   public static final double kSteerRatio = 150.0 / 7.0;
-  private final double kWheelDiameterMeters = Units.inchesToMeters(1.87 * 2);
+  private final double kWheelDiameterMeters = Units.inchesToMeters(1.73 * 2);
   private final double kWheelRadiusMeters = kWheelDiameterMeters / 2.0;
   private final double kCouplingRatio = kDriveRatioFirstStage;
 

src/main/java/frc/robot/shooter/Shooter.java

@@ -131,13 +131,8 @@ public Command trapAssist() {
         .withName("Trap Assist");
   }
 
-  public Command stashShot() {
-    return this.run(
-            () -> {
-              m_io.setTopRollerVoltage(12.0);
-              m_io.setBottomRollerVoltage(12.0);
-            })
-        .withName("Stash Shot (Shooter)");
+  public Command feederShot() {
+    return this.run(() -> setRollersSetpointRpm(3500.0)).withName("Feeder Shot (Shooter)");
   }
 
   public Command ampShot() {

src/main/java/frc/robot/vision/GamePieceDetection.java

@@ -1,19 +1,23 @@
 package frc.robot.vision;
 
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants;
-import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.Logger;
 
 public class GamePieceDetection {
-  private final double kTargetAreaThreshold = 0.0;
+  private final double kMinimumTargetAreaPercent = 1.7;
   private final GamePieceDetectionIO m_io;
   private final GamePieceDetectionIOInputsAutoLogged m_inputs =
       new GamePieceDetectionIOInputsAutoLogged();
 
-  public final BooleanSupplier hasValidTarget =
-      () -> m_inputs.hasTarget || m_inputs.targetArea > kTargetAreaThreshold;
-  public final DoubleSupplier horizontalError =
+  public final Trigger hasValidTarget =
+      new Trigger(
+          () ->
+              m_inputs.connected
+                  && m_inputs.hasTarget
+                  && m_inputs.targetArea > kMinimumTargetAreaPercent);
+  public final DoubleSupplier horizontalErrorDeg =
       () ->
           m_inputs.targetHorizontalOffsetDegrees
               * (Constants.onRedAllianceSupplier.getAsBoolean() ? -1 : 1);
@@ -26,6 +30,6 @@ public void periodic() {
     m_io.updateInputs(m_inputs);
     Logger.processInputs("VisionInputs/Limelight", m_inputs);
     Logger.recordOutput("GamePieceDetection/ValidTarget", hasValidTarget.getAsBoolean());
-    Logger.recordOutput("GamePieceDetection/HorizontalError", horizontalError.getAsDouble());
+    Logger.recordOutput("GamePieceDetection/HorizontalErrorDeg", horizontalErrorDeg.getAsDouble());
   }
 }