Vahay (#138)

* Switch to field April tags

* Added Aapril tag json, removed reef offset, updated climber servo constants

* Remove reef offset todo

* morning tweaks

* Fix elevator rocket

* Move zorro controller command into teleop init.

* sycronized blue and red auto logic

* removed withtimout on elevator

* added timout to both red and blue L4autos

* Set default stop command for swerve in autonomous

* Prevent elevator rocket in joystick command

* made the elevator and robot move at the same time.

* Tweaked the choro by 3 inches

* Widen tolerance for pose seek display

* Remove init from lifter joystick command

* Tweaked the redCenterToL4G choreo

* commented out the createjoystickcontrolcommand and tweaked choreo.

* Another attempt to prevent skyrocket on teleop init

* Adjusted path for 1 piece auto

* Add a few metrics to smart dashboard

* Slowed down the red Auto in Choero

* Adjust Al;gae L3 height, comment out algae wrist set angle on grab

* spotless

---------

Co-authored-by: Pez357 <[email protected]>

Author: caparomula

src/main/deploy/2025-reefscape-andymark.json

@@ -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/deploy/choreo/blueCenterToL4G.traj

@@ -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/deploy/choreo/redCenterToL4G.traj

@@ -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);

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

@@ -48,7 +48,6 @@
 import frc.robot.elevator.AlgaeWrist;
 import frc.robot.elevator.CoralRoller;
 import frc.robot.elevator.Elevator;
-import frc.robot.elevator.ElevatorConstants.AlgaeWristConstants.AlgaeWristState;
 import frc.robot.elevator.Lifter;
 import frc.robot.vision.Vision;
 import java.util.HashMap;
@@ -125,9 +124,6 @@ public void robotInit() {
     DataLogManager.start();
     DriverStation.startDataLog(DataLogManager.getLog());
 
-    swerve.setDefaultCommand(
-        new ZorroDriveCommand(swerve, DriveConstants.kDriveKinematics, driver.getHID()));
-
     reefTargetPositionsPublisher.set(DriveConstants.kReefTargetPoses);
   }
 
@@ -199,6 +195,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 +207,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 +346,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 +386,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());
   }
 

src/main/java/frc/robot/Constants.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/LEDs/LEDs.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/Robot.java

@@ -192,6 +192,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
    */

src/main/java/frc/robot/auto/BlueL4Auto.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/auto/RedL4Auto.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/drivetrain/Drivetrain.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);
 

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

@@ -181,12 +181,16 @@ public Command createSetHeightCommand(LifterState state) {
         this);
   }
 
+  public void matchHeight() {
+    feedback.setGoal(encoder.getPosition());
+  }
+
   public Command createRemainAtCurrentHeightCommand() {
     return new FunctionalCommand(
         // initialize
         () -> {
           if (targetState == LifterState.Initial) {
-            feedback.setGoal(encoder.getPosition());
+            matchHeight();
             // Users should call reset() when they first start running the controller to avoid
             // unwanted behavior.
             resetController();
@@ -209,7 +213,13 @@ private Boolean isInRange(Distance height) {
   }
 
   public Command createJoystickControlCommand(XboxController gamepad) {
-    return this.run(
+    return new FunctionalCommand(
+        // initialize
+        () -> {
+          // matchHeight();
+          // resetController();
+        },
+        // execute
         () -> {
           Distance targetPosition = Meters.of(feedback.getGoal().position);
 
@@ -220,7 +230,13 @@ public Command createJoystickControlCommand(XboxController gamepad) {
 
           if (isInRange(targetPosition)) feedback.setGoal(targetPosition.in(Meters));
           control();
-        });
+        },
+        // end
+        interrupted -> {},
+        // isFinished
+        () -> false,
+        // requirements
+        this);
   }
 
   public Command createJoystickVoltageCommand(XboxController gamepad) {

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

@@ -10,8 +10,6 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import frc.robot.Constants;
-import frc.robot.Constants.VisionConstants;
-import java.io.IOException;
 import java.util.List;
 import java.util.Optional;
 import org.photonvision.EstimatedRobotPose;
@@ -42,17 +40,17 @@ private Camera(String name, Translation3d translation, Rotation3d rotation) {
     this.transform = new Transform3d(translation, rotation);
     this.device = new PhotonCamera(name);
 
-    // TODO: switch back to official field layout
-    AprilTagFieldLayout tagLayout;
-    try {
-      tagLayout = new AprilTagFieldLayout(VisionConstants.kAprilTagLayoutPath);
-    } catch (IOException e) {
-      System.err.println("Error loading custom AprilTag layout: " + e.getMessage());
-      tagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
-    }
+    // // TODO: switch back to official field layout
+    // AprilTagFieldLayout tagLayout;
+    // try {
+    //   tagLayout = new AprilTagFieldLayout(VisionConstants.kAprilTagLayoutPath);
+    // } catch (IOException e) {
+    //   System.err.println("Error loading custom AprilTag layout: " + e.getMessage());
+    //   tagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
+    // }
 
-    // AprilTagFieldLayout tagLayout =
-    //     AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark);
+    AprilTagFieldLayout tagLayout =
+        AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark);
 
     this.pose =
         new PhotonPoseEstimator(tagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, transform);