worked on auto amp

src/main/deploy/pathplanner/paths/l get topcen c.path

@@ -32,7 +32,7 @@
   "constraintZones": [
     {
       "name": "New Constraints Zone",
-      "minWaypointRelativePos": 0.65,
+      "minWaypointRelativePos": 0.9,
       "maxWaypointRelativePos": 1.0,
       "constraints": {
         "maxVelocity": 2.0,

src/main/java/frc/robot/Constants.java

@@ -144,7 +144,7 @@ public static class Presets {
     public static final double ELEVATOR_HEIGHT_RATE = -0.025;
 
     public static final double SHOOTER_AMP_HANDOFF = 50;
-    public static final double ELEVATOR_AMP_HANDOFF = 0.035;
+    public static final double ELEVATOR_AMP_HANDOFF = 0.045;
 
     public static final InterpolatingDoubleTreeMap SHOOTER_DISTANCE_ANGLE = new InterpolatingDoubleTreeMap();
 
@@ -184,7 +184,7 @@ public static class FieldConstants {
 
     public static final double SHOOTER_SLOW_THRESHOLD = 2;
 
-    public static final double TAG_DISTANCE_THRESHOLD = 3.5;
+    public static final double TAG_DISTANCE_THRESHOLD = 3;
     public static final double SINGLE_TAG_DISTANCE_THRESHOLD = 1.5;
 
     public static final int SPEAKER_TAG_BLUE = 7;

src/main/java/frc/robot/RobotContainer.java

@@ -234,7 +234,7 @@ public void teleopInit() {
   /** @return The Command to schedule for auton. */
   public Command getAutonCommand() {
     _swerveSubsystem.fieldOriented = false; // make sure swerve is robot-relative for pathplanner to work
-    _shooterSubsystem.setShooterState(ShooterState.IDLE);
+    _shooterSubsystem.setShooterState(ShooterState.SHOOT);
     _intakeSubsystem.setHasNoteAuton(false);
 
     Command test = new SequentialCommandGroup(

src/main/java/frc/robot/commands/auto/AutoAmp.java

@@ -27,11 +27,13 @@ public AutoAmp(
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
-      new SpinShooter(shooter, ShooterState.AMP, false).withTimeout(0.3),
+      new SpinShooter(shooter, ShooterState.AMP, false).withTimeout(0.15),
       new PrintCommand("REVVED"),
-      new SpinShooter(shooter, ShooterState.NONE, false).withTimeout(0.2),
+      new SpinShooter(shooter, ShooterState.NONE, true),
       new PrintCommand("CAN AMP"),
-      new FeedActuate(intake, ActuatorState.STOWED, FeedMode.OUTTAKE).withTimeout(0.5)
+      new FeedActuate(intake, ActuatorState.STOWED, FeedMode.OUTTAKE).withTimeout(0.5),
+      new SpinShooter(shooter, ShooterState.SLOW, false).withTimeout(0.1)
+
     );
   }
 }

src/main/java/frc/robot/commands/auto/AutonShoot.java

@@ -38,13 +38,13 @@ public AutonShoot(
       // This command will squeeze the note and rev up the shooter if needed, all while auto-aiming.
       new ParallelCommandGroup(
         new SpinShooter(shooter, ShooterState.SHOOT, true).andThen(new WaitUntilCommand(shooter::isRevved)),
-        new FeedActuate(intake, FeedMode.INTAKE).withTimeout(1).onlyIf(() -> !intake.hasNoteAuton()),
+        new FeedActuate(intake, FeedMode.INTAKE).withTimeout(0.5).onlyIf(() -> !intake.hasNoteAuton()),
         new AutoAim(swerve, shooter, elevator, leds)
         // new AutoAim(swerve, shooter, elevator, leds, () -> 0, () -> 50.0, () -> 0.04).andThen(() -> System.out.println("AUTO !! AIMED"))
       ),
 
-      new FeedActuate(intake, FeedMode.OUTTAKE).withTimeout(0.5),
-      new SpinShooter(shooter, ShooterState.IDLE, true)
+      new FeedActuate(intake, FeedMode.OUTTAKE).withTimeout(0.5)
+      // new SpinShooter(shooter, ShooterState.IDLE, true)
     );
   }
 

src/main/java/frc/robot/subsystems/ShooterSubsystem.java

@@ -50,7 +50,7 @@ public class ShooterSubsystem extends SubsystemBase {
 
   private final Timer _revTimer = new Timer();
 
-  private double _shooterTrim = 0;
+  private double _shooterTrim = 3;
 
   private boolean _holdNote = false;
 
@@ -76,6 +76,7 @@ public ShooterSubsystem() {
     _revShooterEncoder.setDistancePerPulse((360.0 / Physical.SHOOTER_ENCODER_ANGLE_GEAR_RATIO)/8192.0);
     _revShooterEncoder.reset();
 
+
     resetAngle();
 
     // soft limits
@@ -89,7 +90,7 @@ public ShooterSubsystem() {
 
     _angleMotor.getConfigurator().apply(softLimits);
 
-    _angleController.setTolerance(1);
+    _angleController.setTolerance(2.5);
 
     SmartDashboard.putNumber("SHOOTER TRIM", _shooterTrim);
   }
@@ -128,7 +129,7 @@ public double speakerAngle() {
 
   /** Returns whether the shooter (motor) is revved up (if enough time has elapsed). */
   public boolean isRevved() {
-    return _revTimer.hasElapsed(1);
+    return _revTimer.hasElapsed(0.75);
   }
 
   /**

src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java

@@ -200,29 +200,34 @@ private void updateBotpose() {
       double tagCount = llBotpose[7];
 
       Pose2d botpose = UtilFuncs.ToPose(llBotpose);
-      double poseDifference = botpose.getTranslation().getDistance(botpose.getTranslation());
+      double poseDifference = botpose.getTranslation().getDistance(getPose().getTranslation());
 
       SmartDashboard.putNumber("DISTANCE TAGG", tagDistance);
 
       double xyStds;
       double yawStd = 9999999;
 
+      System.out.println(poseDifference);
+
       if (tagDistance > FieldConstants.TAG_DISTANCE_THRESHOLD) {
         return;
       }
 
       // good distance, multiple tags
       if (tagCount >= 2) {
+        System.out.println("TWO TAGS");
         xyStds = 0.55;
       }
 
       // one tag, closer distance, estimated pose is inaccurate
       else if (tagDistance <= FieldConstants.SINGLE_TAG_DISTANCE_THRESHOLD && poseDifference <= 1.5) {
+        // System.out.println("ONE TAG CLOSE POSE INNACCURATE");
         xyStds = 0.65;
       }
 
       // one tag, closer distance, estimated pose is accurate
       else if (tagDistance <= FieldConstants.SINGLE_TAG_DISTANCE_THRESHOLD && poseDifference <= 0.5) {
+        // System.out.println("ONE TAG CLOSE POSE ACCURATE");
         xyStds = 0.85;
       }