Jade/devbot

src/main/java/org/curtinfrc/frc2025/Constants.java

@@ -16,11 +16,12 @@
 import static org.curtinfrc.frc2025.subsystems.vision.VisionConstants.aprilTagLayout;
 
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotBase;
-import java.util.ArrayList;
 import java.util.List;
 import org.curtinfrc.frc2025.util.PoseUtil;
+import org.littletonrobotics.junction.Logger;
 
 /**
  * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running
@@ -29,8 +30,8 @@
  */
 public final class Constants {
   public static final RobotType robotType = RobotType.DEVBOT;
-  public static final double ROBOT_X = 550; // mm
-  public static final double ROBOT_Y = 570;
+  public static final double ROBOT_X = 660; // mm
+  public static final double ROBOT_Y = 680;
 
   public static final Mode getMode() {
     return switch (robotType) {
@@ -71,20 +72,22 @@ public static void main(String... args) {
   // TODO: MAKE SETPOINTS
   public enum Setpoints {
     /* in mm */
-    COLLECT(0, List.of(13, 12), List.of(1, 2)),
+    COLLECT(0, List.of(13, 12), List.of(1, 2), false),
     // L1(460, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)), //TODO
     // TODO actually subtract
-    L2(13.2 - 2.5, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6)),
-    L3(35.327, List.of(17, 18, 19, 20, 21, 22), List.of(9, 8, 10, 8, 11, 6));
+    L2(11, List.of(18 /*17, 18, 19, 20, 21, 22*/), List.of(9, 8, 10, 8, 11, 6), true),
+    L3(32.8, List.of(18 /*17, 18, 19, 20, 21, 22*/), List.of(9, 8, 10, 8, 11, 6), true);
 
     private final double _elevatorSetpoint;
     private final List<Integer> _tagIdsBlue;
     private final List<Integer> _tagIdsRed;
+    private final boolean flip;
 
-    Setpoints(double elevator, List<Integer> tagIdsBlue, List<Integer> tagIdsRed) {
+    Setpoints(double elevator, List<Integer> tagIdsBlue, List<Integer> tagIdsRed, boolean flip) {
       this._elevatorSetpoint = elevator;
       this._tagIdsBlue = tagIdsBlue;
       this._tagIdsRed = tagIdsRed;
+      this.flip = flip;
     }
 
     public double elevatorSetpoint() {
@@ -102,20 +105,75 @@ public Pose3d toPose(Pose3d currentPose) {
     private Pose3d resolvePose(List<Integer> tagIds, Pose3d currentPose) {
       if (tagIds.isEmpty()) return new Pose3d();
 
-      ArrayList<Pose3d> poses = new ArrayList<>();
+      double sideOffset = 0.32 / 2;
+
+      class ClosestPose {
+        Pose3d pose = null;
+        double distance = Double.MAX_VALUE;
+      }
+
+      ClosestPose closest = new ClosestPose();
+
       for (int tagId : tagIds) {
         aprilTagLayout
             .getTagPose(tagId)
-            .ifPresent(pose -> poses.add(PoseUtil.mapPose(pose.toPose2d())));
+            .ifPresent(
+                tagPose -> {
+                  Pose3d mappedPose = PoseUtil.mapPose(tagPose);
+                  // mappedPose = mappedPose.rotateBy(new Rotation3d(0, 0, Math.PI));
+                  Rotation3d rotation = mappedPose.getRotation();
+                  double baseAngle = rotation.getAngle(),
+                      cos = Math.cos(baseAngle),
+                      sin = Math.sin(baseAngle);
+                  double xOffset = sideOffset * sin, yOffset = sideOffset * cos;
+
+                  for (Pose3d pose :
+                      new Pose3d[] {
+                        new Pose3d(
+                            mappedPose.getX() + xOffset,
+                            mappedPose.getY() - yOffset,
+                            mappedPose.getZ(),
+                            rotation.rotateBy(new Rotation3d(0, 0, this.flip ? Math.PI : 0))),
+                        new Pose3d(
+                            mappedPose.getX() - xOffset,
+                            mappedPose.getY() + yOffset,
+                            mappedPose.getZ(),
+                            rotation.rotateBy(new Rotation3d(0, 0, this.flip ? Math.PI : 0)))
+                      }) {
+                    double distance = distanceBetween(pose, currentPose);
+                    if (distance < closest.distance) {
+                      closest.pose = pose;
+                      closest.distance = distance;
+                    }
+                  }
+
+                  String tagPrefix = String.format("resolvePose.tagId_%d", tagId);
+                  Logger.recordOutput(tagPrefix + "/baseAngle", String.format("%.2f", baseAngle));
+                  Logger.recordOutput(
+                      tagPrefix + "/leftPose",
+                      formatPose(
+                          new Pose3d(
+                              mappedPose.getX() + xOffset,
+                              mappedPose.getY() - yOffset,
+                              mappedPose.getZ(),
+                              rotation)));
+                  Logger.recordOutput(
+                      tagPrefix + "/rightPose",
+                      formatPose(
+                          new Pose3d(
+                              mappedPose.getX() - xOffset,
+                              mappedPose.getY() + yOffset,
+                              mappedPose.getZ(),
+                              rotation)));
+                  Logger.recordOutput(tagPrefix + "/currentPose", formatPose(currentPose));
+                  Logger.recordOutput(tagPrefix + "/tagPose", formatPose(tagPose));
+                });
       }
-      if (poses.isEmpty()) return new Pose3d();
-
-      return poses.stream()
-          .min(
-              (pose1, pose2) ->
-                  Double.compare(
-                      distanceBetween(pose1, currentPose), distanceBetween(pose2, currentPose)))
-          .orElse(new Pose3d());
+      return closest.pose != null ? closest.pose : new Pose3d();
+    }
+
+    private String formatPose(Pose3d pose) {
+      return String.format("(%.2f, %.2f, %.2f)", pose.getX(), pose.getY(), pose.getZ());
     }
 
     private double distanceBetween(Pose3d pose1, Pose3d pose2) {

src/main/java/org/curtinfrc/frc2025/Robot.java

@@ -269,7 +269,7 @@ public Robot() {
             () -> -controller.getLeftX(),
             () -> -controller.getRightX()));
 
-    // elevator.setDefaultCommand(elevator.goToSetpoint(Setpoints.COLLECT));
+    elevator.setDefaultCommand(elevator.goToSetpoint(Setpoints.COLLECT));
     controller
         .rightBumper()
         .negate()
@@ -279,6 +279,9 @@ public Robot() {
     intake.setDefaultCommand(intake.intake(intakeVolts));
     ejector.setDefaultCommand(ejector.stop());
 
+    // elevator.toZero.whileTrue(intake.intake(intakeVolts));
+    elevator.toZero.negate().whileTrue(intake.stop());
+
     intake
         .backSensor
         .and(intake.frontSensor.negate())
@@ -295,7 +298,16 @@ public Robot() {
         .and(intake.frontSensor.negate())
         .whileTrue(Commands.parallel(intake.stop(), ejector.stop()).withName("not front and back"));
 
-    controller.x().whileTrue(ejector.eject(1000));
+    elevator.toZero.whileTrue(
+        elevator
+            .zero()
+            .until(elevator.toZero.negate())
+            .ignoringDisable(true)
+            .withName("ElevatorZero"));
+    // elevator.isNotAtCollect.negate().whileTrue(new PrintCommand("pls pls work"));
+
+    controller.b().onTrue(elevator.zero());
+    controller.rightTrigger().whileTrue(ejector.eject(1000));
 
     // Lock to 0° when A button is held
     controller
@@ -321,6 +333,11 @@ public Robot() {
     controller
         .leftBumper()
         .whileTrue(Commands.defer(() -> elevator.goToSetpoint(Setpoints.L2), Set.of(elevator)));
+
+    // controller.pov(0).whileTrue(superstructure.align(Setpoints.L1));
+    controller.rightBumper().whileTrue(superstructure.align(Setpoints.L3));
+    controller.leftBumper().whileTrue(superstructure.align(Setpoints.L2));
+    controller.leftTrigger().whileTrue(superstructure.align(Setpoints.COLLECT));
   }
 
   /** This function is called periodically during all modes. */
@@ -347,6 +364,12 @@ public void robotPeriodic() {
 
     autoChooser.periodic();
 
+    if (elevator.getCurrentCommand() != null) {
+      Logger.recordOutput("ElevatorCommand", elevator.getCurrentCommand().getName());
+    } else {
+      Logger.recordOutput("ElevatorCommand", "null");
+    }
+
     // Return to normal thread priority
     Threads.setCurrentThreadPriority(false, 10);
   }

src/main/java/org/curtinfrc/frc2025/Superstructure.java

@@ -23,6 +23,6 @@ public Command align(Setpoints setpoint) {
             Commands.parallel(
                 drive.autoAlign(setpoint.toPose(new Pose3d(drive.getPose()))),
                 elevator.goToSetpoint(setpoint)),
-        Set.of(drive, elevator));
+        Set.of(elevator, drive));
   }
 }

src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java

@@ -76,14 +76,21 @@ public class Drive extends SubsystemBase {
   private SwerveDrivePoseEstimator poseEstimator =
       new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, Pose2d.kZero);
 
+  private double p = 6;
+  private double d = 0.2;
+  private double i = 0.03;
+
   private final PIDController xController = new PIDController(10.0, 0.0, 0.0);
   private final PIDController yController = new PIDController(10.0, 0.0, 0.0);
-  private final PIDController headingController = new PIDController(7.5, 0.0, 0.0);
+  private final PIDController headingController = new PIDController(p, i, d);
 
   private final SlewRateLimiter xLimiter = new SlewRateLimiter(7); // Limits acceleration to 3 mps
   private final SlewRateLimiter yLimiter = new SlewRateLimiter(7);
   // private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(1);
 
+  private final SlewRateLimiter omegaAutoLimiter = new SlewRateLimiter(100);
+  private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(1);
+
   RepulsorFieldPlanner repulsorFieldPlanner = new RepulsorFieldPlanner();
 
   public Drive(
@@ -127,6 +134,10 @@ public Drive(
                 (voltage) -> runSteerCharacterization(voltage.in(Volts)), null, this));
 
     headingController.enableContinuousInput(-Math.PI, Math.PI);
+
+    // SmartDashboard.putNumber("P", 0.9);
+    // SmartDashboard.putNumber("D", 0.02);
+    // SmartDashboard.putNumber("I", 0.01);
   }
 
   @Override
@@ -139,6 +150,10 @@ public void periodic() {
     }
     odometryLock.unlock();
 
+    // headingController.setP(SmartDashboard.getNumber("P", 2));
+    // headingController.setD(SmartDashboard.getNumber("D", 0.01));
+    // headingController.setI(SmartDashboard.getNumber("I", 0));
+
     // Stop moving when disabled
     if (DriverStation.isDisabled()) {
       for (var module : modules) {
@@ -194,6 +209,9 @@ public void periodic() {
    * @param speeds Speeds in meters/sec
    */
   private void runVelocity(ChassisSpeeds speeds) {
+    speeds.vxMetersPerSecond = xLimiter.calculate(speeds.vxMetersPerSecond);
+    speeds.vyMetersPerSecond = yLimiter.calculate(speeds.vyMetersPerSecond);
+
     SwerveModuleState[] setpointStates =
         kinematics.toSwerveModuleStates(ChassisSpeeds.discretize(speeds, 0.02));
     SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, TunerConstants.kSpeedAt12Volts);
@@ -292,8 +310,8 @@ public Command joystickDrive(
 
           ChassisSpeeds speeds =
               new ChassisSpeeds(
-                  xLimiter.calculate(linearVelocity.getX() * getMaxLinearSpeedMetersPerSec()),
-                  yLimiter.calculate(linearVelocity.getY() * getMaxLinearSpeedMetersPerSec()),
+                  linearVelocity.getX() * getMaxLinearSpeedMetersPerSec(),
+                  linearVelocity.getY() * getMaxLinearSpeedMetersPerSec(),
                   omega * getMaxAngularSpeedRadPerSec());
 
           // Logger.recordOutput("Drive/OmegaLimited", limited);
@@ -307,7 +325,6 @@ public Command joystickDrive(
                   speeds, isFlipped ? getRotation().plus(Rotation2d.kPi) : getRotation()));
         });
   }
-
   /**
    * Field relative drive command using joystick for linear control and PID for angular control.
    * Possible use cases include snapping to an angle, aiming at a vision target, or controlling
@@ -358,6 +375,9 @@ public void followTrajectory(SwerveSample sample) {
     // Get the current pose of the robot
     Pose2d pose = getPose();
     Logger.recordOutput("Odometry/TrajectorySetpoint", pose);
+    Logger.recordOutput("Drive/PID/error", headingController.getError());
+    var out = headingController.calculate(pose.getRotation().getRadians(), sample.heading);
+    Logger.recordOutput("Drive/PID/out", out);
 
     // Generate the next speeds for the robot
     ChassisSpeeds speeds =
@@ -366,9 +386,8 @@ public void followTrajectory(SwerveSample sample) {
             sample.vy + yController.calculate(pose.getY(), sample.y),
             sample.omega
                 + headingController.calculate(pose.getRotation().getRadians(), sample.heading),
-            getRotation());
+            getRotation()); // Apply the generated speeds
 
-    // Apply the generated speeds
     runVelocity(speeds);
   }
 
@@ -624,10 +643,10 @@ public Command autoAlign(Pose3d _setpoint) {
                   true);
 
           // Include rotational alignment using the existing heading controller
-          double adjustedOmega =
-              headingController.calculate(
-                  robotPose.getRotation().getRadians(),
-                  _setpoint.toPose2d().getRotation().getRadians());
+          // double adjustedOmega =
+          //     headingController.calculate(
+          //         robotPose.getRotation().getRadians(),
+          //         _setpoint.toPose2d().getRotation().getRadians());
 
           // Apply the trajectory with rotation adjustment
           SwerveSample adjustedSample =
@@ -638,7 +657,8 @@ public Command autoAlign(Pose3d _setpoint) {
                   _setpoint.toPose2d().getRotation().getRadians(),
                   cmd.vx,
                   cmd.vy,
-                  adjustedOmega,
+                  // adjustedOmega,
+                  0,
                   cmd.ax,
                   cmd.ay,
                   cmd.alpha,

src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java

@@ -15,7 +15,8 @@ public class Elevator extends SubsystemBase {
   private final PIDController pid = new PIDController(kP, 0, kD);
   private Setpoints setpoint = Setpoints.COLLECT;
 
-  public final Trigger isNotAtCollect = new Trigger(() -> setpoint != Setpoints.COLLECT);
+  public final Trigger isNotAtCollect = new Trigger(() -> inputs.point != Setpoints.COLLECT);
+  public final Trigger toZero = new Trigger(() -> inputs.touchSensor);
 
   public Elevator(ElevatorIO io) {
     this.io = io;
@@ -25,10 +26,13 @@ public Elevator(ElevatorIO io) {
   public void periodic() {
     io.updateInputs(inputs);
     Logger.processInputs("Elevator", inputs);
+    Logger.recordOutput("Elevator/toZero", toZero.getAsBoolean());
     Logger.recordOutput("Elevator/setpoint", setpoint);
-    Logger.recordOutput("Elevator/isNotAtCollect", isNotAtCollect);
+    // Logger.recordOutput("Elevator/isNotAtCollect", isNotAtCollect);
   }
 
+  // public Trigger atSetpoint = new Trigger(pid::atSetpoint);
+
   public Trigger atSetpoint = new Trigger(pid::atSetpoint);
 
   public Command goToSetpoint(Setpoints point) {
@@ -42,6 +46,14 @@ public Command goToSetpoint(Setpoints point) {
         });
   }
 
+  public Command zero() {
+    return run(() -> io.zero());
+  }
+
+  // public Command goToSetpoint(Setpoints point) {
+  //   return run(() -> io.goToSetpoint(point));
+  // }
+
   public Command stop() {
     return runOnce(() -> io.setVoltage(0));
   }