Align closed loop

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

@@ -356,7 +356,7 @@ public static AngularVelocity metersToRotationVel(double metersPerSecond){
      public class RobotK {
         public static final String kLogTab = "SuperStructure";
         // TODO: get a real distance from the reef for this
-        public static final double kRobotCenterDistanceFromReef = Units.inchesToMeters(-17);
+        public static final double kRobotCenterDistanceFromReef = Units.inchesToMeters(-16.5);
         public static final double kRobotScoringOffset = Units.inchesToMeters(2.9); // positive left robot, measured 4/1/2025
         public static final Transform2d kTransformReefPoseToRobotPosition = new Transform2d(kRobotCenterDistanceFromReef, kRobotScoringOffset, Rotation2d.fromDegrees(0));
      }
@@ -453,7 +453,7 @@ public static class SharedAutoAlignK {
         public static final Distance kFieldTranslationTolerance = Meters.of(0.025); // meters
         public static final Angle kFieldRotationTolerance = Degrees.of(0.5); // degrees
 
-        public static final double kIntermediatePoseDistance = -Units.inchesToMeters(6); // value in meters
+        public static final double kIntermediatePoseDistance = -Units.inchesToMeters(8); // value in meters
         public static final Transform2d kIntermediatePoseTransform 
             = new Transform2d(kIntermediatePoseDistance, 0, Rotation2d.kZero);
 
@@ -511,8 +511,8 @@ public static class LegacyAutoAlignK {
     public static class MovingAutoAlignK {
         public static final String kLogTab = "MovingAutoAlign";
 
-        public static double kXKP = 8;
-        public static double kYKP = 8;
+        public static double kXKP = 8.5;
+        public static double kYKP = 8.5;
         public static double kThetaKP = 10;
 
         // SUPER COOL AUTO ALIGN :sunglasses: - this should eventually allow you to replace all code using above constants

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

@@ -10,6 +10,8 @@
 import java.util.List;
 import java.util.Optional;
 import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.targeting.PhotonTrackedTarget;
 
 import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
@@ -46,6 +48,8 @@
 import frc.robot.autons.TrajsAndLocs.HPStation;
 import frc.robot.autons.TrajsAndLocs.ReefLocs;
 import frc.robot.autons.TrajsAndLocs.StartingLocs;
+
+import static frc.robot.Constants.kTestingAutonOnCart;
 import static frc.robot.autons.TrajsAndLocs.ReefLocs.*;
 
 import frc.robot.autons.WaltAutonFactory;
@@ -94,6 +98,7 @@ public class Robot extends TimedRobot {
     VisionK.kElevatorForwardsCamRoboToCam, visionSim, VisionK.kEleForwardCamSimProps);
   private final Vision lowerRightCam = new Vision(VisionK.kLowerRightCamName, VisionK.kLowerRightCamSimVisualName,
     VisionK.kLowerRightCamRoboToCam, visionSim, VisionK.kLowerRightCamSimProps);
+  private final int[] tagsToUse = {6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22};
 
   // this should be updated with all of our cameras
   private final Vision[] cameras = {eleForwardsCam, lowerRightCam};  // lower right cam removed readded and ready to rumble
@@ -444,6 +449,20 @@ public void robotPeriodic() {
       if (estimatedPoseOptional.isPresent()) {
         EstimatedRobotPose estimatedRobotPose = estimatedPoseOptional.get();
         Pose2d estimatedRobotPose2d = estimatedRobotPose.estimatedPose.toPose2d();
+        boolean invalidEstimation = false;
+        // if it isn't multitag
+        if (estimatedRobotPose.strategy != PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR) {
+          // loop through used targets
+          for (PhotonTrackedTarget target : estimatedRobotPose.targetsUsed) {
+            // if any of them are invalid targets set invalidEstimation flag to true
+            if (!isUsedTagId(target.getFiducialId())) { invalidEstimation = true; }
+          }
+        }
+
+        if (invalidEstimation == true) {
+          continue;
+        }
+
         var ctreTime = Utils.fpgaToCurrentTime(estimatedRobotPose.timestampSeconds);
         drivetrain.addVisionMeasurement(estimatedRobotPose2d, ctreTime, camera.getEstimationStdDevs());
         lastGotTagMsmtTimer.restart();
@@ -483,24 +502,42 @@ public void disabledPeriodic() {
 
       // --- PRESET AUTONS
       if (WaltAutonBuilder.nte_rightThreePiece.getBoolean(false)) {
-        waltAutonFactory = Optional.of(autonFactoryFactory(
-          StartingLocs.RIGHT, 
-          List.of(REEF_E, REEF_D, REEF_C, REEF_B), 
-          List.of(EleHeight.L4, EleHeight.L4, EleHeight.L4, EleHeight.L4),
-          List.of(HPStation.HP_RIGHT, HPStation.HP_RIGHT, HPStation.HP_RIGHT, HPStation.HP_RIGHT)
+        if(kTestingAutonOnCart) {
+          waltAutonFactory = Optional.of(autonFactoryFactory(
+            StartingLocs.RIGHT, 
+            List.of(REEF_E, REEF_D, REEF_C, REEF_B), 
+            List.of(EleHeight.L2, EleHeight.L2, EleHeight.L2, EleHeight.L2),
+            List.of(HPStation.HP_RIGHT, HPStation.HP_RIGHT, HPStation.HP_RIGHT, HPStation.HP_RIGHT)
+        ));
+        } else {
+          waltAutonFactory = Optional.of(autonFactoryFactory(
+            StartingLocs.RIGHT, 
+            List.of(REEF_E, REEF_D, REEF_C, REEF_B), 
+            List.of(EleHeight.L4, EleHeight.L4, EleHeight.L4, EleHeight.L4),
+            List.of(HPStation.HP_RIGHT, HPStation.HP_RIGHT, HPStation.HP_RIGHT, HPStation.HP_RIGHT)
         ));
+        }
 
         Elastic.sendNotification(new Elastic.Notification(NotificationLevel.INFO, "Auton Path DEFINED", "Right 3 piece auton generated"));
         WaltAutonBuilder.nte_rightThreePiece.setBoolean(false);
       } 
 
       if (WaltAutonBuilder.nte_leftThreePiece.getBoolean(false)) {
-        waltAutonFactory = Optional.of(autonFactoryFactory(
-          StartingLocs.LEFT, 
-          List.of(REEF_J, REEF_K, REEF_L, REEF_A),
-          List.of(EleHeight.L4, EleHeight.L4, EleHeight.L4, EleHeight.L4),
-          List.of(HPStation.HP_LEFT, HPStation.HP_LEFT, HPStation.HP_LEFT, HPStation.HP_LEFT)
+        if(kTestingAutonOnCart) {
+          waltAutonFactory = Optional.of(autonFactoryFactory(
+            StartingLocs.LEFT, 
+            List.of(REEF_J, REEF_K, REEF_L, REEF_A),
+            List.of(EleHeight.L2, EleHeight.L2, EleHeight.L2, EleHeight.L2),
+            List.of(HPStation.HP_LEFT, HPStation.HP_LEFT, HPStation.HP_LEFT, HPStation.HP_LEFT)
         ));
+        } else {
+          waltAutonFactory = Optional.of(autonFactoryFactory(
+            StartingLocs.LEFT, 
+            List.of(REEF_J, REEF_K, REEF_L, REEF_A),
+            List.of(EleHeight.L4, EleHeight.L4, EleHeight.L4, EleHeight.L4),
+            List.of(HPStation.HP_LEFT, HPStation.HP_LEFT, HPStation.HP_LEFT, HPStation.HP_LEFT)
+        ));
+        }
 
         // autonName = "Left 3 Piece: ";
         Elastic.sendNotification(new Elastic.Notification(NotificationLevel.INFO, "Auton Path DEFINED", "Left 3 piece auton generated"));
@@ -521,6 +558,18 @@ public void disabledPeriodic() {
         WaltAutonBuilder.nte_midOnePiece.setBoolean(false);
       }
 
+      if(WaltAutonBuilder.nte_midTown.getBoolean(false)) {
+        waltAutonFactory = Optional.of(autonFactoryFactory(
+          StartingLocs.MID_G,
+          List.of(REEF_G, REEF_D, REEF_E),
+          List.of(EleHeight.L4, EleHeight.L4, EleHeight.L4),
+          List.of(HPStation.HP_RIGHT, HPStation.HP_RIGHT, HPStation.HP_RIGHT)
+        ));
+
+        Elastic.sendNotification(new Elastic.Notification(NotificationLevel.INFO, "Auton Path DEFINED", "Mid 3 piece auton generated"));
+        WaltAutonBuilder.nte_midTown.setBoolean(false);
+      }
+
 
       // fail-case (no auton selected) - do nothing (its no longer that now)
       if (readyToMakeAuton && waltAutonFactory.isEmpty()) {
@@ -576,6 +625,15 @@ public void disabledExit() {
 
   }
 
+  public boolean isUsedTagId(int tagId) {
+    for (int i = 0; i < tagsToUse.length; i++) {
+      if (tagsToUse[i] == tagId) {
+        return true;
+      }
+    }
+    return false;
+  }
+
   private Command autonCmdBuilder(Command chooserCommand) {
     return Commands.parallel(
           Commands.print("running autonCmdBuilder"),

src/main/java/frc/robot/autoalign/LegacyAutoAlign.java

@@ -3,6 +3,7 @@
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue;
 
@@ -32,7 +33,8 @@ public class LegacyAutoAlign {
 
 
     private static final SwerveRequest.FieldCentric swreq_driveFieldCentricBlue = new SwerveRequest.FieldCentric()
-        .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance);
+        .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance)
+        .withDriveRequestType(DriveRequestType.Velocity);
 
     public static Command moveToPose(Swerve drivetrain, Supplier<Pose2d> destinationPose) {
         final Pose2d[] cachedTarget = {Pose2d.kZero};

src/main/java/frc/robot/autoalign/MovingAutoAlign.java

@@ -3,6 +3,7 @@
 import java.util.function.Supplier;
 
 import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
 import edu.wpi.first.math.MathUtil;
@@ -112,15 +113,16 @@ public static Command moveToPose(
         headingController.enableContinuousInput(-Math.PI, Math.PI);
         // ok, use passed constraints on X controller
         final ProfiledPIDController vxController =
-            new ProfiledPIDController(MovingAutoAlignK.kXKP, 0.01, 0.015, xyConstraints.get());
+            new ProfiledPIDController(MovingAutoAlignK.kXKP, 0.01, 0.01, xyConstraints.get());
         // use constraints from constants for y controller?
         // why define them with different constraints?? it's literally field relative
         // the difference in x and y dimensions almost definitely do not mean anything to robot movement
         final ProfiledPIDController vyController =
-            new ProfiledPIDController(MovingAutoAlignK.kYKP, 0.01, 0.015, xyConstraints.get());
+            new ProfiledPIDController(MovingAutoAlignK.kYKP, 0.01, 0.01, xyConstraints.get());
 
         // this is created at trigger binding, not created every time the command is scheduled
-        final SwerveRequest.ApplyFieldSpeeds swreq_driveFieldSpeeds = new SwerveRequest.ApplyFieldSpeeds();
+        final SwerveRequest.ApplyFieldSpeeds swreq_driveFieldSpeeds = new SwerveRequest.ApplyFieldSpeeds()
+            .withDriveRequestType(DriveRequestType.Velocity);
 
         return Commands.runOnce(
             () -> {                

src/main/java/frc/robot/autons/TrajsAndLocs.java

@@ -201,6 +201,7 @@ public static class Trajectories {
             ReefToHPTrajs.put(new Pair<ReefLocs, HPStation>(ReefLocs.REEF_D, HPStation.HP_RIGHT), "D_Right");
             ReefToHPTrajs.put(new Pair<ReefLocs, HPStation>(ReefLocs.REEF_E, HPStation.HP_RIGHT), "E_Right");
             ReefToHPTrajs.put(new Pair<ReefLocs, HPStation>(ReefLocs.REEF_F, HPStation.HP_RIGHT), "F_Right");
+            ReefToHPTrajs.put(new Pair<ReefLocs, HPStation>(ReefLocs.REEF_G, HPStation.HP_RIGHT), "G_Right");
             ReefToHPShortTrajs.put(new Pair<ReefLocs, HPStation>(ReefLocs.REEF_A, HPStation.HP_RIGHT), "A_Right_short");
             ReefToHPShortTrajs.put(new Pair<ReefLocs, HPStation>(ReefLocs.REEF_B, HPStation.HP_RIGHT), "B_Right_short");
             ReefToHPShortTrajs.put(new Pair<ReefLocs, HPStation>(ReefLocs.REEF_C, HPStation.HP_RIGHT), "C_Right_short");

src/main/java/frc/robot/autons/WaltAutonBuilder.java

@@ -17,7 +17,7 @@ public class WaltAutonBuilder {
     public static GenericEntry nte_customAutonReady;
     public static GenericEntry nte_autonRobotPush;  // button to select if we are pushing another robot b4 starting path
     public static GenericEntry nte_clearAll;
-
+   
     public static GenericEntry nte_autonReadyToGo;  // to let the user know that an auton is loaded
     public static GenericEntry nte_autonName;
 
@@ -26,6 +26,8 @@ public class WaltAutonBuilder {
     public static GenericEntry nte_rightThreePiece;
     public static GenericEntry nte_leftThreePiece;
     public static GenericEntry nte_midOnePiece;
+    public static GenericEntry nte_midTown;
+
     // public static GenericEntry nte_midGOnly;
 
     // ---- Initial
@@ -101,6 +103,11 @@ public class WaltAutonBuilder {
                 .add("Mid 1 Piece", false)
                 .withWidget(BuiltInWidgets.kToggleSwitch)
                 .getEntry();
+
+        nte_midTown = Shuffleboard.getTab("AutonChooser")
+                .add("Mid 3 Piece", false)
+                .withWidget(BuiltInWidgets.kToggleSwitch)
+                .getEntry();
 
     }
 

src/main/java/frc/robot/generated/TunerConstants.java

@@ -29,7 +29,7 @@ public class TunerConstants {
     // When using closed-loop control, the drive motor uses the control
     // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
     private static final Slot0Configs driveGains = new Slot0Configs()
-        .withKP(0.35).withKI(0).withKD(0)
+        .withKP(0.25).withKI(0).withKD(0)
         .withKS(0.2034625).withKV(0.1232525).withKA(0.0085018);
 
     // The closed-loop output type to use for the steer motors;

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

@@ -326,6 +326,7 @@ public void simulationPeriodic() {
     }
 
     private static final double kInch = 0.169;
+    private static final double kEighthInch = kInch / 8;
 
     public enum EleHeight {
         HOME(0.3),

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

@@ -460,9 +460,8 @@ private void configureStateActions() {
                     m_coral.slowScore(),
                     Commands.waitSeconds(0.1845),
                     m_ele.toHeightCoral(() -> L2),
-                    m_finger.l1HelperCmd(),
                     Commands.waitSeconds(0.18),
-                    m_finger.inCmd()
+                    m_coral.stopCmd()
                 )
             );