Convert cameras to subsystems, add vision-only pose estimate for pose-seek

src/main/java/frc/game/Reef.java

@@ -18,8 +18,7 @@
  * to a pose.
  */
 public enum Reef {
-  // TODO remove reef ceter offset
-  Blue(new Pose2d(Inches.of(176.75), Inches.of(158.5 + 4.5), Rotation2d.kZero)),
+  Blue(new Pose2d(Inches.of(176.75), Inches.of(158.5), Rotation2d.kZero)),
   Red(new Pose2d(Inches.of(514.125), Inches.of(158.5), Rotation2d.kPi));
 
   /** The radius of the reef hexagon */
@@ -75,10 +74,10 @@ public int getSector(Pose2d atPose) {
     var delta = centerPose.getTranslation().minus(atPose.getTranslation());
     int vectorAngle = (int) delta.getAngle().getDegrees();
     int rayAngle = (vectorAngle + 30 + 360 + (int) centerPose.getRotation().getDegrees()) % 360;
-    SmartDashboard.putNumber("Sector delta X", delta.getX());
-    SmartDashboard.putNumber("Sector delta Y", delta.getY());
-    SmartDashboard.putNumber("Sector vector delta", vectorAngle);
-    SmartDashboard.putNumber("Sector ray angle", rayAngle);
+    // SmartDashboard.putNumber("Sector delta X", delta.getX());
+    // SmartDashboard.putNumber("Sector delta Y", delta.getY());
+    // SmartDashboard.putNumber("Sector vector delta", vectorAngle);
+    // SmartDashboard.putNumber("Sector ray angle", rayAngle);
     return rayAngle / 60;
   }
 

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

@@ -14,7 +14,6 @@
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.units.measure.Time;
-import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.util.Color;
 import frc.game.FeederStation;
@@ -39,9 +38,6 @@ public static final class NEOVortexConstants {
   }
 
   public static final class VisionConstants {
-    public static final String kAprilTagLayoutPath =
-        Filesystem.getDeployDirectory() + "/" + "stemgym.json";
-
     // Define the standard deviations for the pose estimator, which determine how fast the pose
     // estimate converges to the vision measurement. This should depend on the vision measurement
     // noise and how many or how frequently vision measurements are applied to the pose estimator.
@@ -228,8 +224,8 @@ public static final class RotationControllerGains {
   public static final class ClimberConstants {
     public static final int kClimberPort = 17;
     public static final int kRatchetServoPort = 1;
-    public static final double kEngagedPosition = 0 / 1024.0;
-    public static final double kDisengedPosition = 1024.0 / 1024.0;
+    public static final double kEngagedPosition = 800 / 1024.0;
+    public static final double kDisengedPosition = 950 / 1024.0;
 
     public static final int kCageSensorPort = 6;
 

src/main/java/frc/robot/LEDs/LEDs.java

@@ -326,10 +326,10 @@ public void displayPoseSeek(Pose2d currentPose, Pose2d targetPose) {
         Segments.MIDDLE);
 
     var x = delta.getTranslation().getMeasureX().in(Centimeters);
-    fill(Math.abs(x) < 3 ? Color.kWhite : x > 0 ? Color.kGreen : Color.kRed, Segments.TOP);
+    fill(Math.abs(x) < 5 ? Color.kWhite : x > 0 ? Color.kGreen : Color.kRed, Segments.TOP);
 
     var y = delta.getTranslation().getMeasureY().in(Centimeters);
-    if (Math.abs(y) < 3) {
+    if (Math.abs(y) < 6) {
       fill(Color.kWhite, Segments.BOTTOM);
     } else {
       fill(Color.kGreen, y > 0 ? Segments.leftBottom : Segments.rightBottom);
@@ -371,18 +371,6 @@ public Command createStandardDisplayCommand(
         () -> displayDefaultInfo(isAlgeMode.getAsBoolean(), Optional.ofNullable(gamepiece.get())));
   }
 
-  /**
-   * Create a command to display pose seeking information on the LEDs.
-   *
-   * @param targetPoseSupplier provides the target pose
-   * @param currentPoseSupplier provides the current pose
-   * @return a command to display pose seeking information
-   */
-  public Command createPoseSeekingCommand(
-      Supplier<Pose2d> targetPoseSupplier, Supplier<Pose2d> currentPoseSupplier) {
-    return newCommand(() -> displayPoseSeek(currentPoseSupplier.get(), targetPoseSupplier.get()));
-  }
-
   /**
    * Create a pattern to display stacked blocks on the left and right strips.
    *
@@ -446,8 +434,10 @@ public void displayAutoMode(
          */
         auto -> {
           var hasInitialPose = auto.getInitialPose().isPresent();
-          if (hasInitialPose) {
+          if (hasInitialPose && currentPose != null) {
             displayPoseSeek(currentPose, auto.getInitialPose().get());
+          } else if (currentPose == null) {
+            fill(Color.kDarkGray, Segments.ALL);
           }
           displayAutoSelection(
               auto.getAllianceColor(), auto.getOptionNumber(), agreement, hasInitialPose);

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

@@ -1,5 +1,7 @@
 package frc.robot;
 
+import static edu.wpi.first.units.Units.Seconds;
+
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.StructArrayPublisher;
@@ -115,7 +117,15 @@ public Gamepiece get() {
         "Align Encoders",
         new InstantCommand(() -> swerve.zeroAbsTurningEncoderOffsets()).ignoringDisable(true));
 
-    addPeriodic(() -> swerve.refreshRelativeTurningEncoder(), 0.1);
+    addPeriodic(() -> swerve.refreshRelativeTurningEncoder(), Seconds.of(0.1));
+    // TODO: see what happens with and without this odometry update
+    // addPeriodic(() -> updateOdometry(), Seconds.of(1));
+  }
+
+  public void updateOdometry() {
+    vision
+        .getEstimatedGlobalPose()
+        .ifPresent(pose -> swerve.resetOdometry(pose.estimatedPose.toPose2d()));
   }
 
   @Override
@@ -125,9 +135,6 @@ public void robotInit() {
     DataLogManager.start();
     DriverStation.startDataLog(DataLogManager.getLog());
 
-    swerve.setDefaultCommand(
-        new ZorroDriveCommand(swerve, DriveConstants.kDriveKinematics, driver.getHID()));
-
     reefTargetPositionsPublisher.set(DriveConstants.kReefTargetPoses);
   }
 
@@ -166,7 +173,10 @@ public void disabledInit() {
     leds.replaceDefaultCommandImmediately(
         leds.createAutoOptionDisplayCommand(
                 autoSelector,
-                () -> swerve.getPose(),
+                () ->
+                    vision.getEstimatedGlobalPose().isPresent()
+                        ? vision.getEstimatedGlobalPose().get().estimatedPose.toPose2d()
+                        : null,
                 allianceSelector.getAgreementInAllianceColor())
             .ignoringDisable(true));
 
@@ -199,6 +209,7 @@ public void disabledPeriodic() {
   @Override
   public void autonomousInit() {
     autoSelector.scheduleAuto();
+    swerve.setDefaultCommand(swerve.createStopCommand());
     lifter.setDefaultCommand(lifter.createRemainAtCurrentHeightCommand());
     leds.replaceDefaultCommandImmediately(
         leds.createStandardDisplayCommand(algaeModeSupplier, gamepieceSupplier));
@@ -210,7 +221,13 @@ public void autonomousPeriodic() {}
   @Override
   public void teleopInit() {
     autoSelector.cancelAuto();
+    swerve.setDefaultCommand(
+        new ZorroDriveCommand(swerve, DriveConstants.kDriveKinematics, driver.getHID()));
+
+    lifter.matchHeight();
+    lifter.resetController();
     lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID()));
+    // lifter.setDefaultCommand(lifter.createJoystickControlCommand(operator.getHID()));
     leds.replaceDefaultCommandImmediately(
         leds.createStandardDisplayCommand(algaeModeSupplier, gamepieceSupplier));
 
@@ -343,7 +360,7 @@ public boolean getAsBoolean() {
     // Force joystick operation of the elevator
     Trigger elevatorTriggerHigh = operator.axisGreaterThan(Axis.kLeftY.value, 0.9, loop).debounce(0.1);
     Trigger elevatorTriggerLow = operator.axisGreaterThan(Axis.kLeftY.value, -0.9, loop).debounce(0.1);
-    elevatorTriggerHigh.or(elevatorTriggerLow).onTrue(lifter.createJoystickControlCommand(operator.getHID()));
+    // elevatorTriggerHigh.or(elevatorTriggerLow).onTrue(lifter.createJoystickControlCommand(operator.getHID()));
 
     // Actuate climber winch
     // Trigger climbTrigger = operator.axisGreaterThan(Axis.kRightY.value, -0.9, loop).debounce(0.1);
@@ -383,8 +400,8 @@ private void configureEventBindings() {
 
     algaeRoller.hasAlgae
         .whileTrue(algaeRoller.createHoldAlgaeCommand());
-    algaeRoller.hasAlgae
-        .onTrue(algaeWrist.createSetAngleCommand(AlgaeWristState.Barge));
+    // algaeRoller.hasAlgae
+    //     .onTrue(algaeWrist.createSetAngleCommand(AlgaeWristState.Barge));
     coralRoller.isRolling.or(algaeRoller.isRolling).whileTrue(createRollerAnimationCommand());
   }
 
@@ -429,6 +446,7 @@ private synchronized StructPublisher<Pose2d> getPose2dPublisher(String name) {
   }
 
   protected void checkVision() {
+
     vision
         .getPoseEstimates()
         .forEach(

src/main/java/frc/robot/auto/AutoMode.java

@@ -7,15 +7,13 @@
 import java.util.Optional;
 
 public abstract class AutoMode {
-  private final Drivetrain drivetrain;
   private final AutoFactory autoFactory;
 
-  public AutoMode(Drivetrain dt) {
-    this.drivetrain = dt;
+  public AutoMode(Drivetrain drivetrain) {
     autoFactory =
         new AutoFactory(
             drivetrain::getPose,
-            drivetrain::setPose,
+            drivetrain::resetOdometry,
             drivetrain::followTrajectory,
             false,
             drivetrain);

src/main/java/frc/robot/auto/BlueL4Auto.java

@@ -48,9 +48,7 @@ public AutoRoutine getAutoRoutine() {
     blueL4AutoRoutine.active().onTrue(
       Commands.parallel(
         blueCenterToL4G.cmd(),
-        Commands.sequence(
-          Commands.waitSeconds(1.0),
-          elevator.coralL4PositionCG())));
+        elevator.coralL4PositionCG().withTimeout(2.0)));
 
     blueCenterToL4G.done().onTrue(
         Commands.sequence(

src/main/java/frc/robot/auto/RedL4Auto.java

@@ -45,13 +45,15 @@ public Optional<Pose2d> getInitialPose() {
   public AutoRoutine getAutoRoutine() {
 
     // spotless:off
-    redL4AutoRoutine.active().onTrue(redCenterToL4G.cmd());
+    redL4AutoRoutine.active().onTrue(
+      Commands.parallel(
+        redCenterToL4G.cmd(),
+        elevator.coralL4PositionCG().withTimeout(2.0)));
+
 
     redCenterToL4G.done().onTrue(
         Commands.sequence(
-            Commands.waitSeconds(0.1),
-            elevator.coralL4PositionCG(),
-            Commands.waitSeconds(0.1),
+            Commands.waitSeconds(0.1), 
             coralRoller.createOuttakeCommand().withTimeout(0.2),
             Commands.waitSeconds(0.2)));
 

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

@@ -11,6 +11,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.numbers.N1;
@@ -47,6 +48,7 @@ public class Drivetrain extends SubsystemBase {
 
   // The robot pose estimator for tracking swerve odometry and applying vision corrections.
   private final SwerveDrivePoseEstimator poseEstimator;
+  private final SwerveDriveOdometry odometry;
 
   private final PIDController xController =
       new PIDController(TranslationControllerGains.kP, 0.0, 0.0);
@@ -58,8 +60,10 @@ public class Drivetrain extends SubsystemBase {
   private final Canandgyro canandgyro = new Canandgyro(0);
   private Rotation2d headingOffset = Rotation2d.kZero;
 
-  private StructPublisher<Pose2d> m_visionPosePublisher =
+  private StructPublisher<Pose2d> visionPosePublisher =
       NetworkTableInstance.getDefault().getStructTopic("Vision", Pose2d.struct).publish();
+  private StructPublisher<Pose2d> odometryPosePublisher =
+      NetworkTableInstance.getDefault().getStructTopic("Odometry", Pose2d.struct).publish();
 
   public Drivetrain(BooleanSupplier fieldRotated, Supplier<Dimensionless> elevatorHeight) {
     this.fieldRotatedSupplier = fieldRotated;
@@ -81,14 +85,21 @@ public Drivetrain(BooleanSupplier fieldRotated, Supplier<Dimensionless> elevator
             Pose2d.kZero,
             DriveConstants.kStateStdDevs,
             VisionConstants.kMultiTagStdDevs);
+    odometry =
+        new SwerveDriveOdometry(
+            DriveConstants.kDriveKinematics,
+            canandgyro.getRotation2d(),
+            getSwerveModulePositions());
 
     thetaController.enableContinuousInput(-Math.PI, Math.PI);
   }
 
   @Override
   public void periodic() {
     poseEstimator.update(canandgyro.getRotation2d(), getSwerveModulePositions());
-    m_visionPosePublisher.set(poseEstimator.getEstimatedPosition());
+    odometry.update(canandgyro.getRotation2d(), getSwerveModulePositions());
+    visionPosePublisher.set(poseEstimator.getEstimatedPosition());
+    odometryPosePublisher.set(odometry.getPoseMeters());
 
     for (SwerveModule module : SwerveModule.values()) {
       SmartDashboard.putNumber(
@@ -169,8 +180,20 @@ public Pose2d getPose() {
   /**
    * @param pose The robot pose
    */
-  public void setPose(Pose2d pose) {
+  public void resetOdometry(Pose2d pose) {
+    resetOdometry(pose, false);
+  }
+
+  /**
+   * Reset the swerve drive's field pose.
+   *
+   * @param pose New robot pose.
+   * @param resetSimPose If the simulated robot pose should also be reset. This effectively
+   *     teleports the robot and should only be used during the setup of the simulation world.
+   */
+  public void resetOdometry(Pose2d pose, boolean resetSimPose) {
     poseEstimator.resetPosition(canandgyro.getRotation2d(), getSwerveModulePositions(), pose);
+    odometry.resetPosition(canandgyro.getRotation2d(), getSwerveModulePositions(), pose);
   }
 
   public void initializeRelativeTurningEncoder() {
@@ -192,6 +215,10 @@ public void setHeadingOffset() {
         fieldRotatedSupplier.getAsBoolean() ? getHeading().rotateBy(Rotation2d.kPi) : getHeading();
   }
 
+  public Command createStopCommand() {
+    return this.run(() -> setRobotRelativeChassisSpeeds(new ChassisSpeeds()));
+  }
+
   /**
    * @return Array of swerve module positions
    */
@@ -232,17 +259,6 @@ public void addVisionMeasurement(
     poseEstimator.addVisionMeasurement(visionMeasurement, timestampSeconds, stdDevs);
   }
 
-  /**
-   * Reset the estimated pose of the swerve drive on the field.
-   *
-   * @param pose New robot pose.
-   * @param resetSimPose If the simulated robot pose should also be reset. This effectively
-   *     teleports the robot and should only be used during the setup of the simulation world.
-   */
-  public void resetPose(Pose2d pose, boolean resetSimPose) {
-    poseEstimator.resetPosition(canandgyro.getRotation2d(), getSwerveModulePositions(), pose);
-  }
-
   public void followTrajectory(SwerveSample sample) {
     Pose2d pose = getPose();
 

src/main/java/frc/robot/elevator/AlgaeRoller.java

@@ -67,8 +67,8 @@ public AlgaeRoller() {
   @Override
   public void periodic() {
     SmartDashboard.putNumber("Algae Roller/Velocity", encoder.getVelocity());
-    // SmartDashboard.putNumber("Algae Roller/Applied Duty Cycle", leaderMotor.getAppliedOutput());
-    // SmartDashboard.putNumber("Algae Roller/Current", leaderMotor.getOutputCurrent());
+    SmartDashboard.putNumber("Algae Roller/Applied Duty Cycle", leaderMotor.getAppliedOutput());
+    SmartDashboard.putNumber("Algae Roller/Current", leaderMotor.getOutputCurrent());
     SmartDashboard.putBoolean("Algae Loaded", hasAlgae.getAsBoolean());
     SmartDashboard.putBoolean("Algae isRolling", isRolling.getAsBoolean());
   }

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

@@ -49,6 +49,7 @@ public AlgaeWrist getAlgaeWrist() {
   public Command resetPositionControllers() {
     return new InstantCommand(
         () -> {
+          lifter.matchHeight();
           lifter.resetController();
           coralWrist.resetController();
           algaeWrist.resetController();

src/main/java/frc/robot/elevator/ElevatorConstants.java

@@ -73,7 +73,7 @@ public static enum LifterState {
       CoralIntake(11.7),
       AlgaeProcessor(12.0),
       AlgaeL2(29.5),
-      AlgaeL3(45),
+      AlgaeL3(43),
       AlgaeBarge(67.8),
       Max(68.3);