Merge branch 'main' of https://github.com/Shenzhen-Robotics-Alliance/…

…Maple-Swerve-Skeleton

build.gradle

@@ -29,6 +29,15 @@ deploy {
                 // getTargetTypeClass is a shortcut to get the class type using a string
 
                 frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
+                    jvmArgs.add("-XX:+HeapDumpOnOutOfMemoryError")
+                    jvmArgs.add("-XX:HeapDumpPath=/U/frc-usercode.hprof")
+                    // Enable VisualVM connection
+                    jvmArgs.add("-Dcom.sun.management.jmxremote=true")
+                    jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198")
+                    jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false")
+                    jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false")
+                    jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false")
+                    jvmArgs.add("-Djava.rmi.server.hostname=172.22.11.2")
                 }
 
                 // Static files artifact

src/main/deploy/configs/PhotonCamerasProperties/6433-2024-OffSeason.xml

@@ -13,8 +13,8 @@
         <cameraFOVDegrees type="double">60</cameraFOVDegrees>
         <calibrationAverageErrorPixel type="double">0.15</calibrationAverageErrorPixel>
         <calibrationErrorStandardDeviationPixel type="double">0.05</calibrationErrorStandardDeviationPixel>
-        <mountPositionToRobotCenterForwardsMeters type="double">0</mountPositionToRobotCenterForwardsMeters>
-        <mountPositionToRobotCenterLeftwardsMeters type="double">0</mountPositionToRobotCenterLeftwardsMeters>
+        <mountPositionToRobotCenterForwardsMeters type="double">0.265</mountPositionToRobotCenterForwardsMeters>
+        <mountPositionToRobotCenterLeftwardsMeters type="double">0.19</mountPositionToRobotCenterLeftwardsMeters>
         <mountHeightMeters type="double">0.2</mountHeightMeters>
         <cameraPitchDegrees type="double">30</cameraPitchDegrees>
         <cameraYawDegrees type="double">0</cameraYawDegrees>
@@ -29,8 +29,8 @@
         <cameraFOVDegrees type="double">60</cameraFOVDegrees>
         <calibrationAverageErrorPixel type="double">0.15</calibrationAverageErrorPixel>
         <calibrationErrorStandardDeviationPixel type="double">0.05</calibrationErrorStandardDeviationPixel>
-        <mountPositionToRobotCenterForwardsMeters type="double">0</mountPositionToRobotCenterForwardsMeters>
-        <mountPositionToRobotCenterLeftwardsMeters type="double">0</mountPositionToRobotCenterLeftwardsMeters>
+        <mountPositionToRobotCenterForwardsMeters type="double">-0.115</mountPositionToRobotCenterForwardsMeters>
+        <mountPositionToRobotCenterLeftwardsMeters type="double">0.29</mountPositionToRobotCenterLeftwardsMeters>
         <mountHeightMeters type="double">0.2</mountHeightMeters>
         <cameraPitchDegrees type="double">45</cameraPitchDegrees>
         <cameraYawDegrees type="double">45</cameraYawDegrees>
@@ -45,8 +45,8 @@
         <cameraFOVDegrees type="double">60</cameraFOVDegrees>
         <calibrationAverageErrorPixel type="double">0.15</calibrationAverageErrorPixel>
         <calibrationErrorStandardDeviationPixel type="double">0.05</calibrationErrorStandardDeviationPixel>
-        <mountPositionToRobotCenterForwardsMeters type="double">0</mountPositionToRobotCenterForwardsMeters>
-        <mountPositionToRobotCenterLeftwardsMeters type="double">0</mountPositionToRobotCenterLeftwardsMeters>
+        <mountPositionToRobotCenterForwardsMeters type="double">-0.115</mountPositionToRobotCenterForwardsMeters>
+        <mountPositionToRobotCenterLeftwardsMeters type="double">-0.29</mountPositionToRobotCenterLeftwardsMeters>
         <mountHeightMeters type="double">0.2</mountHeightMeters>
         <cameraPitchDegrees type="double">45</cameraPitchDegrees>
         <cameraYawDegrees type="double">-45</cameraYawDegrees>

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

@@ -210,11 +210,13 @@ public static final class VisionConfigs {
         /* default standard error for vision observation, if only one apriltag observed */
         public static final double
                 TRANSLATIONAL_STANDARD_ERROR_METERS_FOR_SINGLE_OBSERVATION = 0.6,
-                ROTATIONAL_STANDARD_ERROR_RADIANS_FOR_SINGLE_OBSERVATION = 0.9,
+                ROTATIONAL_STANDARD_ERROR_RADIANS_FOR_SINGLE_OBSERVATION = 0.5,
+
                 // only do odometry calibration if translational standard error if it is not greater than
-                TRANSLATIONAL_STANDARD_ERROR_THRESHOLD = 0.6,
+                TRANSLATIONAL_STANDARD_ERROR_THRESHOLD = 0.5,
                 // only do gyro calibration if rotational standard error is very, very small
                 ROTATIONAL_STANDARD_ERROR_THRESHOLD = Math.toRadians(5),
+
                 TRANSLATIONAL_STANDARD_ERROR_METERS_ODOMETRY = 0.05,
                 // we trust the IMU very much (recommend 0.1 for Pigeon2, 0.5 for NavX)
                 ROTATIONAL_STANDARD_ERROR_RADIANS_ODOMETRY = Math.toRadians(0.1);

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

@@ -15,7 +15,7 @@
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
 public class Robot extends LoggedRobot {
-    private static final Constants.RobotMode JAVA_SIM_MODE = Constants.RobotMode.SIM;
+    private static final Constants.RobotMode JAVA_SIM_MODE = Constants.RobotMode.REPLAY;
     public static final Constants.RobotMode CURRENT_ROBOT_MODE = isReal() ? Constants.RobotMode.REAL : JAVA_SIM_MODE;
     private Command autonomousCommand;
     private RobotContainer robotContainer;

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

@@ -20,7 +20,9 @@
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.autos.Auto;
 import frc.robot.autos.AutoBuilder;
+import frc.robot.commands.drive.AutoAlignment;
 import frc.robot.commands.drive.CustomFollowPath;
+import frc.robot.commands.drive.CustomFollowPathOnFly;
 import frc.robot.commands.drive.JoystickDrive;
 import frc.robot.subsystems.drive.*;
 import frc.robot.subsystems.drive.IO.GyroIOPigeon2;
@@ -224,15 +226,13 @@ private void configureButtonBindings() {
                 ).ignoringDisable(true)
         );
 
-        driverController.y().whileTrue(new CustomFollowPath(
+        driverController.y().whileTrue(new AutoAlignment(
                 drive,
-                MaplePathPlannerLoader.fromPathFileReversed(
-                        "Test Path",
-                        new PathConstraints(3, 6, 6.28, 10),
-                        new GoalEndState(0, new Rotation2d())
-                ).flipPath(),
-                "Drive/TestPath"
-        ).rePlannedOnStart());
+                () -> Constants.toCurrentAlliancePose(new Pose2d(1.85, 7.35, Rotation2d.fromDegrees(-90))),
+                () -> Constants.toCurrentAlliancePose(new Pose2d(1.85, 7.7, Rotation2d.fromDegrees(-90))),
+                new Pose2d(0.1, 0.1, Rotation2d.fromDegrees(3)),
+                0.5
+        ));
     }
 
     /**

src/main/java/frc/robot/commands/drive/AutoAlignment.java

@@ -15,7 +15,7 @@
 public class AutoAlignment extends SequentialCommandGroup {
     private static final Pose2d DEFAULT_TOLERANCE = new Pose2d(0.03, 0.03, new Rotation2d(2));
     public AutoAlignment(HolonomicDriveSubsystem driveSubsystem, Supplier<Pose2d> targetPose){
-        this(driveSubsystem, targetPose, DEFAULT_TOLERANCE);
+        this(driveSubsystem, targetPose, targetPose, DEFAULT_TOLERANCE, 0.75);
     }
 
     /**
@@ -25,11 +25,11 @@ public AutoAlignment(HolonomicDriveSubsystem driveSubsystem, Supplier<Pose2d> ta
      * 1. path-find to the target pose, roughly
      * 2. accurate auto alignment
      * */
-    public AutoAlignment(HolonomicDriveSubsystem driveSubsystem, Supplier<Pose2d> targetPose, Pose2d tolerance) {
-        final Command pathFindToTargetRough = new PathFindToPose(driveSubsystem, targetPose, 0.75),
+    public AutoAlignment(HolonomicDriveSubsystem driveSubsystem, Supplier<Pose2d> roughTarget, Supplier<Pose2d> target, Pose2d tolerance, double speedMultiplier) {
+        final Command pathFindToTargetRough = new PathFindToPose(driveSubsystem, target, speedMultiplier),
                 preciseAlignment = new DriveToPosition(
                         driveSubsystem,
-                        targetPose,
+                        target,
                         tolerance
                 );
 
@@ -38,36 +38,4 @@ public AutoAlignment(HolonomicDriveSubsystem driveSubsystem, Supplier<Pose2d> ta
 
         super.addRequirements(driveSubsystem);
     }
-
-    public AutoAlignment(
-            PathConstraints constraints,
-            HolonomicDriveController holonomicDriveController,
-            Supplier<Pose2d> robotPoseSupplier,
-            Consumer<ChassisSpeeds> robotRelativeSpeedsOutput,
-            Subsystem driveSubsystem,
-            Pose2d targetPose,
-            Pose2d tolerance
-    ) {
-        /* tolerance for the precise approach */
-        holonomicDriveController.setTolerance(tolerance);
-        final Command
-                pathFindToTargetRough = AutoBuilder.pathfindToPose(targetPose, constraints, 0.5),
-                preciseAlignment = new FunctionalCommand(
-                        () -> {},
-                        () -> robotRelativeSpeedsOutput.accept(holonomicDriveController.calculate(
-                                robotPoseSupplier.get(),
-                                targetPose,
-                                0,
-                                targetPose.getRotation()
-                        )),
-                        (interrupted) ->
-                                robotRelativeSpeedsOutput.accept(new ChassisSpeeds()),
-                        holonomicDriveController::atReference
-                        );
-
-        super.addCommands(pathFindToTargetRough);
-        super.addCommands(preciseAlignment);
-
-        super.addRequirements(driveSubsystem);
-    }
 }

src/main/java/frc/robot/commands/drive/JoystickDrive.java

@@ -45,9 +45,9 @@ public void initialize() {
         this.previousChassisUsageTimer.reset();
         this.previousRotationalInputTimer.reset();
         this.currentPilotInputSpeeds = new ChassisSpeeds();
-        this.currentRotationMaintenanceSetpoint = driveSubsystem.getFacing();
+        this.currentRotationMaintenanceSetpoint = driveSubsystem.getRawGyroYaw();
 
-        this.chassisRotationController.calculate(driveSubsystem.getFacing().getRadians()); // activate controller
+        this.chassisRotationController.calculate(driveSubsystem.getRawGyroYaw().getRadians()); // activate controller
     }
 
     @Override
@@ -76,15 +76,18 @@ public void execute() {
             currentPilotInputSpeeds = new ChassisSpeeds();
 
         final ChassisSpeeds chassisSpeedsWithRotationMaintenance;
-        final double rotationCorrectionAngularVelocity = chassisRotationController.calculate(driveSubsystem.getFacing().getRadians(), currentRotationMaintenanceSetpoint.getRadians());
+        final double rotationCorrectionAngularVelocity = chassisRotationController.calculate(
+                driveSubsystem.getRawGyroYaw().getRadians(),
+                currentRotationMaintenanceSetpoint.getRadians()
+        );
         if (previousRotationalInputTimer.get() > Constants.DriveConfigs.timeActivateRotationMaintenanceAfterNoRotationalInputSeconds)
             chassisSpeedsWithRotationMaintenance = new ChassisSpeeds(
                     currentPilotInputSpeeds.vxMetersPerSecond, currentPilotInputSpeeds.vyMetersPerSecond,
                     rotationCorrectionAngularVelocity
             );
         else {
             chassisSpeedsWithRotationMaintenance = currentPilotInputSpeeds;
-            currentRotationMaintenanceSetpoint = driveSubsystem.getFacing();
+            currentRotationMaintenanceSetpoint = driveSubsystem.getRawGyroYaw();
         }
 
         Logger.recordOutput("JoystickDrive/rotation maintenance set-point (deg)", currentRotationMaintenanceSetpoint.getDegrees());

src/main/java/frc/robot/subsystems/drive/HolonomicDriveSubsystem.java

@@ -35,6 +35,8 @@ public interface HolonomicDriveSubsystem extends Subsystem {
 
     default Rotation2d getFacing() {return getPose().getRotation(); }
 
+    default Rotation2d getRawGyroYaw() {return getFacing(); }
+
     /**
      * Resets the current odometry Pose to a given Pose
      */

src/main/java/frc/robot/subsystems/drive/SwerveDrive.java

@@ -230,6 +230,8 @@ private SwerveModulePosition[] getModuleLatestPositions() {
     public Pose2d getPose() {
         return poseEstimator.getEstimatedPosition();
     }
+    @Override
+    public Rotation2d getRawGyroYaw() {return gyroInputs.yawPosition; }
 
     @Override
     public void setPose(Pose2d pose) {

src/main/java/frc/robot/subsystems/vision/apriltags/AprilTagVision.java

@@ -1,10 +1,11 @@
 package frc.robot.subsystems.vision.apriltags;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.subsystems.MapleSubsystem;
 import frc.robot.subsystems.drive.HolonomicDriveSubsystem;
 import frc.robot.utils.Config.PhotonCameraProperties;
 import org.littletonrobotics.junction.Logger;
-import org.photonvision.targeting.PhotonPipelineResult;
 
 import java.util.List;
 import java.util.Optional;
@@ -27,7 +28,7 @@ public AprilTagVision(AprilTagVisionIO io, List<PhotonCameraProperties> camerasP
 
         this.multiTagPoseEstimator = new MapleMultiTagPoseEstimator(
                 fieldLayout,
-                new CameraHeightFilter(),
+                new CameraHeightAndPitchRollAngleFilter(),
                 camerasProperties
         );
         this.driveSubsystem = driveSubsystem;
@@ -44,20 +45,51 @@ public void periodic(double dt, boolean enabled) {
         Logger.processInputs(APRIL_TAGS_VISION_PATH + "Inputs", inputs);
 
         Optional<RobotPoseEstimationResult> result = multiTagPoseEstimator.estimateRobotPose(inputs.camerasInputs, driveSubsystem.getPose());
+        result = discardResultIfOverThreshold(result);
         result.ifPresent(robotPoseEstimationResult -> driveSubsystem.addVisionMeasurement(
                 robotPoseEstimationResult.pointEstimation,
                 getResultsTimeStamp(),
                 robotPoseEstimationResult.estimationStandardError
         ));
 
-        Logger.recordOutput(
-                APRIL_TAGS_VISION_PATH + "Results/Estimated Pose",
-                result.map(robotPoseEstimationResult -> robotPoseEstimationResult.pointEstimation).orElse(null)
-        );
         Logger.recordOutput(
                 APRIL_TAGS_VISION_PATH + "Results/Standard Error",
                 result.map(printStandardError).orElse(null)
         );
+
+        Logger.recordOutput(APRIL_TAGS_VISION_PATH + "Results/Estimated Pose", displayVisionPointEstimateResult(result));
+    }
+
+    private Optional<RobotPoseEstimationResult> discardResultIfOverThreshold(Optional<RobotPoseEstimationResult> result) {
+        if (result.isEmpty())
+            return result;
+
+        double standardDeviationX = result.get().translationXStandardDeviationMeters,
+                standardDeviationY = result.get().translationYStandardDeviationMeters,
+                standardDeviationTheta = result.get().rotationalStandardDeviationRadians;
+        /* don't calibrate odometry if translation error is not inside range */
+        if (standardDeviationX > TRANSLATIONAL_STANDARD_ERROR_THRESHOLD || standardDeviationY > TRANSLATIONAL_STANDARD_ERROR_THRESHOLD)
+            standardDeviationTheta = standardDeviationX = standardDeviationY = Double.POSITIVE_INFINITY;
+        /* don't calibrate gyro if rotation error is not inside range */
+        if (standardDeviationTheta > ROTATIONAL_STANDARD_ERROR_THRESHOLD)
+            standardDeviationTheta = Double.POSITIVE_INFINITY;
+
+        return Optional.of(new RobotPoseEstimationResult(
+                result.get().pointEstimation,
+                standardDeviationX,
+                standardDeviationY,
+                standardDeviationTheta
+        ));
+    }
+
+    private Pose2d displayVisionPointEstimateResult(Optional<RobotPoseEstimationResult> result) {
+        if (result.isEmpty()
+                || Double.isInfinite(result.get().translationXStandardDeviationMeters)
+                || Double.isInfinite(result.get().translationYStandardDeviationMeters))
+            return null;
+        if (Double.isInfinite(result.get().rotationalStandardDeviationRadians))
+            return new Pose2d(result.get().pointEstimation.getTranslation(), driveSubsystem.getFacing());
+        return result.get().pointEstimation;
     }
 
     private double getResultsTimeStamp() {