fine tuned the drivetrain pids

src/main/deploy/choreo/grab sixth and shoot.traj

@@ -82,6 +82,5 @@
     {"t":2.6072148262330437, "x":3.900952100753784, "y":5.60628080368042, "heading":3.141592653589793, "vx":0.0, "vy":0.0, "omega":0.0, "ax":11.241231668708654, "ay":7.457031618684653, "alpha":-0.20225908819860608, "fx":[111.57544211985864,112.58500129044012,113.23875308269538,112.250070255352], "fy":[75.82418489615657,74.31544288372957,73.3176641188492,74.82397284865081]}],
   "splits":[]
  },
- "events":[],
- "pplib_commands":[]
+ "events":[]
 }

src/main/deploy/choreo/shoot third and fourth.traj

@@ -77,6 +77,5 @@
     {"t":2.2229604277583737, "x":3.002295970916748, "y":6.939965724945068, "heading":-2.704135164645048, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-12.785672543246733, "ay":-4.301998901297384, "alpha":0.006409431260137176, "fx":[-127.84703842576408,-127.86883944817907,-127.8664088501156,-127.84461500581048], "fy":[-43.048701920816214,-42.98391881232034,-42.99129291253959,-43.056042406219184]}],
   "splits":[]
  },
- "events":[],
- "pplib_commands":[]
+ "events":[]
 }

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

@@ -36,7 +36,7 @@ public void initialize() {
     @Override
     public void execute() {
         final double rotationFeedBack = chassisRotationController.calculate(driveSubsystem.getFacing().getRadians(), targetRotationSupplier.get().getRadians());
-        driveSubsystem.runRobotCentricChassisSpeeds(new ChassisSpeeds(0, 0, rotationFeedBack));
+        driveSubsystem.runRobotCentricChassisSpeeds(new ChassisSpeeds(0, 0, rotationFeedBack), false);
     }
 
     @Override

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

@@ -56,7 +56,7 @@ public void execute() {
             feedBackSpeeds.vxMetersPerSecond *= speedConstrainMPS / feedBackSpeedMagnitude;
             feedBackSpeeds.vyMetersPerSecond *= speedConstrainMPS / feedBackSpeedMagnitude;
         }
-        driveSubsystem.runRobotCentricChassisSpeeds(feedBackSpeeds);
+        driveSubsystem.runRobotCentricChassisSpeeds(feedBackSpeeds, false);
     }
 
     @Override

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

@@ -49,7 +49,7 @@ public JoystickDrive(MapleJoystickDriveInput input, BooleanSupplier useDriverSta
         this.previousRotationalInputTimer.start();
 
         this.rotationProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(
-                driveSubsystem.getChassisMaxAngularVelocity(),
+                driveSubsystem.getChassisMaxAngularVelocity() * 0.7,
                 driveSubsystem.getChassisMaxAngularAccelerationRadPerSecSq()
         ));
         this.chassisRotationController = new MaplePIDController(DriveControlLoops.CHASSIS_ROTATION_CLOSE_LOOP);
@@ -139,9 +139,9 @@ public void execute() {
         }
 
         if (useDriverStationCentricSwitch.getAsBoolean())
-            driveSubsystem.runDriverStationCentricChassisSpeeds(chassisSpeedsWithRotationMaintenance);
+            driveSubsystem.runDriverStationCentricChassisSpeeds(chassisSpeedsWithRotationMaintenance, true);
         else
-            driveSubsystem.runRobotCentricChassisSpeeds(chassisSpeedsWithRotationMaintenance);
+            driveSubsystem.runRobotCentricChassisSpeeds(chassisSpeedsWithRotationMaintenance, true);
 
         Logger.recordOutput("rotationMaintain", new Pose2d(
                 driveSubsystem.getPose().getTranslation(),

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

@@ -1,5 +1,6 @@
 package frc.robot.commands.drive;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -9,6 +10,7 @@
 import frc.robot.Robot;
 import frc.robot.constants.DriveControlLoops;
 import frc.robot.subsystems.drive.HolonomicDriveSubsystem;
+import frc.robot.subsystems.drive.SwerveDrive;
 import frc.robot.utils.MapleJoystickDriveInput;
 import frc.robot.utils.MapleShooterOptimization;
 import frc.robot.utils.CustomPIDs.MaplePIDController;
@@ -50,6 +52,7 @@ public JoystickDriveAndAimAtTarget(MapleJoystickDriveInput input, HolonomicDrive
     public void initialize() {
         this.chassisRotationController.calculate(driveSubsystem.getRawGyroYaw().getRadians());
         this.chassisRotationInPosition = false;
+        SwerveDrive.acceptRotationalMeasurement = false;
     }
 
     @Override
@@ -62,11 +65,11 @@ public void execute() {
                         getRotationalCorrectionVelocityRadPerSec()
                 ));
 
-        driveSubsystem.runDriverStationCentricChassisSpeeds(chassisSpeeds);
+        driveSubsystem.runDriverStationCentricChassisSpeeds(chassisSpeeds, false);
         super.execute();
     }
 
-    public static double FEED_FORWARD_RATE = 0.5, ROTATION_TOLERANCE_DEGREES = 2.5;
+    public static double FEED_FORWARD_RATE = 1;
     public double getRotationalCorrectionVelocityRadPerSec() {
         final Translation2d robotPosition = driveSubsystem.getPose().getTranslation();
         final ChassisSpeeds robotVelocityFieldRelative = driveSubsystem.getMeasuredChassisSpeedsFieldRelative();
@@ -90,23 +93,28 @@ public double getRotationalCorrectionVelocityRadPerSec() {
         final double targetedFacingChangeRateRadPerSec =
                 targetedFacingAfterDT.minus(targetedFacing).getRadians()
                         / Robot.defaultPeriodSecs;
-        Logger.recordOutput("Drive/Face To Target Rotation (Deg)", targetedFacing.getDegrees());
 
         final double feedBackRotationalSpeed = chassisRotationController.calculate(
                 driveSubsystem.getFacing().getRadians(),
-                targetedFacing.getRadians()),
-                feedForwardRotationalSpeed = targetedFacingChangeRateRadPerSec
-                        * FEED_FORWARD_RATE;
-
-        final double chassisRotationalError = Math.abs(
-                targetedFacing
-                        .minus(driveSubsystem.getFacing())
-                        .getDegrees()
-        );
-        Logger.recordOutput("Drive/Aim At Target Rational Error (Deg)", chassisRotationalError);
-        this.chassisRotationInPosition = chassisRotationalError < ROTATION_TOLERANCE_DEGREES;
+                targetedFacing.getRadians()
+        ),
+                feedForwardRotationalSpeed = MathUtil.applyDeadband(
+                        targetedFacingChangeRateRadPerSec * FEED_FORWARD_RATE,
+                        Math.toRadians(30),
+                        Double.POSITIVE_INFINITY
+                );
+
+        final Rotation2d chassisRotationalError = targetedFacing
+                .minus(driveSubsystem.getFacing());
+        this.chassisRotationInPosition = Math.abs(chassisRotationalError.getRadians())
+                < DriveControlLoops.CHASSIS_ROTATION_CLOSE_LOOP.errorTolerance;
+
         SmartDashboard.putBoolean("Chassis Rotation Aiming Target Reached", chassisRotationInPosition);
-        SmartDashboard.putNumber("Chassis Rotation Aiming Error", chassisRotationalError);
+        SmartDashboard.putNumber("Chassis Rotation Aiming Error", chassisRotationalError.getDegrees());
+        Logger.recordOutput("DriveAndAimAtTarget/Aim At Target Rational Error (Deg)", chassisRotationalError.getDegrees());
+        Logger.recordOutput("DriveAndAimAtTarget/Rotation Target (Deg)", targetedFacing.getDegrees());
+        Logger.recordOutput("DriveAndAimAtTarget/FeedForwardSpeed (Deg per Sec)", Math.toDegrees(feedForwardRotationalSpeed));
+        Logger.recordOutput("DriveAndAimAtTarget/FeedBackSpeed (Deg per Sec)", Math.toDegrees(feedBackRotationalSpeed));
 
         return feedForwardRotationalSpeed + feedBackRotationalSpeed;
     }
@@ -115,4 +123,9 @@ public double getRotationalCorrectionVelocityRadPerSec() {
     public boolean chassisRotationInPosition() {
         return chassisRotationInPosition;
     }
+
+    @Override
+    public void end(boolean interrupted) {
+        SwerveDrive.acceptRotationalMeasurement = true;
+    }
 }

src/main/java/frc/robot/constants/DriveControlLoops.java

@@ -7,11 +7,11 @@
 
 public class DriveControlLoops {
     public static final MaplePIDController.MaplePIDConfig CHASSIS_ROTATION_CLOSE_LOOP = new MaplePIDController.MaplePIDConfig(
-            Math.toRadians(540),
-            Math.toRadians(45),
-            0.01,
+            Math.toRadians(400),
+            Math.toRadians(90),
+            0.03,
             Math.toRadians(3),
-            0.07,
+            0.04,
             true,
             0
     );
@@ -28,7 +28,7 @@ public class DriveControlLoops {
     public static final MaplePIDController.MaplePIDConfig STEER_CLOSE_LOOP = new MaplePIDController.MaplePIDConfig(
             0.5,
             Math.toRadians(90),
-            0.02,
+            0,
             Math.toRadians(1.5),
             0,
             true,

src/main/java/frc/robot/constants/DriveTrainConstants.java

@@ -81,7 +81,7 @@ public class DriveTrainConstants {
                     MAX_PROPELLING_FORCE_NEWTONS / ROBOT_MASS_KG
             ),
             CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC = CHASSIS_MAX_VELOCITY / DRIVE_BASE_RADIUS,
-            CHASSIS_MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ = CHASSIS_MAX_ACCELERATION_MPS_SQ / DRIVE_BASE_RADIUS * 3;
+            CHASSIS_MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ = CHASSIS_MAX_ACCELERATION_MPS_SQ / DRIVE_BASE_RADIUS * 2;
 
     public static final SwerveDriveKinematics DRIVE_KINEMATICS = new SwerveDriveKinematics(MODULE_TRANSLATIONS);
 

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

@@ -94,8 +94,8 @@ public class TunerConstants {
     public static final double kSteerInertia = 0.00001;
     public static final double kDriveInertia = 0.001;
     // Simulated voltage necessary to overcome friction
-    public static final double kSteerFrictionVoltage = 0.25;
-    public static final double kDriveFrictionVoltage = 0.25;
+    public static final double kSteerFrictionVoltage = 0.15;
+    public static final double kDriveFrictionVoltage = 0.2;
 
     public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
             .withCANbusName(kCANbusName)

src/main/java/frc/robot/constants/VisionConstants.java

@@ -23,8 +23,8 @@ public class VisionConstants {
             ROTATIONAL_STANDARD_DEVS_THRESHOLD_DISCARD_RESULT = Math.toRadians(20),
 
             /* standard deviation for odometry and gyros */
-            ODOMETRY_TRANSLATIONAL_STANDARD_ERROR_METERS = 0.08,
-            GYRO_ROTATIONAL_STANDARD_ERROR_RADIANS = Math.toRadians(0.5);
+            ODOMETRY_TRANSLATIONAL_STANDARD_ERROR_METERS = 0.04,
+            GYRO_ROTATIONAL_STANDARD_ERROR_RADIANS = Math.toRadians(0.1);
 
 
     public static final List<PhotonCameraProperties> photonVisionCameras = List.of(

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

@@ -21,6 +21,7 @@
 import frc.robot.Robot;
 import frc.robot.constants.DriveTrainConstants;
 import frc.robot.constants.FieldConstants;
+import frc.robot.subsystems.vision.apriltags.MapleMultiTagPoseEstimator;
 import frc.robot.utils.LocalADStarAK;
 import org.ironmaple.utils.FieldMirroringUtils;
 import org.littletonrobotics.junction.Logger;
@@ -54,11 +55,10 @@ public interface HolonomicDriveSubsystem extends Subsystem {
     /**
      * Adds a vision measurement to the pose estimator.
      *
-     * @param visionPose The pose of the robot as measured by the vision camera.
+     * @param poseEstimationResult the pose estimation result
      * @param timestamp  The timestamp of the vision measurement in seconds.
-     * @param measurementStdDevs the standard deviation of the measurement
      */
-    default void addVisionMeasurement(Pose2d visionPose, double timestamp, Matrix<N3, N1> measurementStdDevs) {}
+    default void addVisionMeasurement(MapleMultiTagPoseEstimator.RobotPoseEstimationResult poseEstimationResult, double timestamp) {}
 
     /**
      * @return the measured(actual) velocities of the chassis, robot-relative
@@ -87,40 +87,37 @@ default PathConstraints getChassisConstrains(double speedMultiplier) {
      * runs a driverstation-centric ChassisSpeeds
      * @param driverStationCentricSpeeds a continuous chassis speeds, driverstation-centric, normally from a gamepad
      * */
-    default void runDriverStationCentricChassisSpeeds(ChassisSpeeds driverStationCentricSpeeds) {
+    default void runDriverStationCentricChassisSpeeds(ChassisSpeeds driverStationCentricSpeeds, boolean discretize) {
         final Rotation2d driverStationFacing = FieldConstants.getDriverStationFacing();
         runRobotCentricChassisSpeeds(ChassisSpeeds.fromFieldRelativeSpeeds(
                 driverStationCentricSpeeds,
                 getPose().getRotation().minus(driverStationFacing)
-        ));
+        ), discretize);
     }
 
     /**
      * runs a field-centric ChassisSpeeds
      * @param fieldCentricSpeeds a continuous chassis speeds, field-centric, normally from a pid position controller
      * */
-    default void runFieldCentricChassisSpeeds(ChassisSpeeds fieldCentricSpeeds) {
+    default void runFieldCentricChassisSpeeds(ChassisSpeeds fieldCentricSpeeds, boolean discretize) {
         runRobotCentricChassisSpeeds(ChassisSpeeds.fromFieldRelativeSpeeds(
                 fieldCentricSpeeds,
                 getPose().getRotation()
-        ));
+        ), discretize);
     }
 
     default void stop() {
-        runRobotCentricChassisSpeeds(new ChassisSpeeds());
+        runRobotCentricChassisSpeeds(new ChassisSpeeds(), false);
     }
 
     /**
      * runs a ChassisSpeeds, pre-processed with ChassisSpeeds.discretize()
      * @param speeds a continuous chassis speed, robot-centric
      * */
-    default void runRobotCentricChassisSpeeds(ChassisSpeeds speeds) {
-        final double PERCENT_DEADBAND = 0.02;
-        if (Math.abs(speeds.omegaRadiansPerSecond) < PERCENT_DEADBAND * getChassisMaxAngularVelocity()
-            && Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond) < PERCENT_DEADBAND * getChassisMaxLinearVelocityMetersPerSec())
-            speeds = new ChassisSpeeds();
-
-        runRawChassisSpeeds(ChassisSpeeds.discretize(speeds, Robot.defaultPeriodSecs));
+    default void runRobotCentricChassisSpeeds(ChassisSpeeds speeds, boolean discretize) {
+        if (discretize)
+            speeds = ChassisSpeeds.discretize(speeds, Robot.defaultPeriodSecs);
+        runRawChassisSpeeds(speeds);
     }
 
     default void configHolonomicPathPlannerAutoBuilder() {

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

@@ -4,7 +4,6 @@
 
 package frc.robot.subsystems.drive;
 
-import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -14,12 +13,11 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.math.numbers.N1;
-import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.constants.FieldConstants;
 import frc.robot.subsystems.MapleSubsystem;
 import frc.robot.subsystems.drive.IO.*;
+import frc.robot.subsystems.vision.apriltags.MapleMultiTagPoseEstimator;
 import frc.robot.utils.Alert;
 import frc.robot.utils.MapleTimeUtils;
 import org.littletonrobotics.junction.AutoLogOutput;
@@ -242,10 +240,13 @@ public ChassisSpeeds getMeasuredChassisSpeedsRobotRelative() {
     @Override public double getChassisMaxAngularVelocity() {return CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC;}
     @Override public double getChassisMaxAngularAccelerationRadPerSecSq() {return CHASSIS_MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ;}
 
+    public static boolean acceptRotationalMeasurement = true;
     @Override
-    public void addVisionMeasurement(Pose2d visionPose, double timestamp, Matrix<N3, N1> measurementStdDevs) {
-        poseEstimator.addVisionMeasurement(visionPose, timestamp ,measurementStdDevs);
+    public void addVisionMeasurement(MapleMultiTagPoseEstimator.RobotPoseEstimationResult poseEstimationResult, double timestamp) {
         previousMeasurementTimeStamp = Math.max(timestamp, previousMeasurementTimeStamp);
+        if (!acceptRotationalMeasurement)
+            poseEstimationResult.rotationalStandardDeviationRadians = 100;
+        poseEstimator.addVisionMeasurement(poseEstimationResult.pointEstimation, timestamp, poseEstimationResult.getEstimationStandardError());
     }
 
     private double previousMeasurementTimeStamp = -1;

src/main/java/frc/robot/utils/CustomPIDs/MaplePIDController.java

@@ -19,15 +19,18 @@ public MaplePIDController(
 
     @Override
     public double calculate(double measurement) {
-        return MathUtil.clamp(
-                MathUtil.applyDeadband(super.calculate(measurement), config.deadBand)
-                , -config.maximumPower, config.maximumPower);
+        final double constrainedCorrection = MathUtil.clamp(super.calculate(measurement), -config.maximumPower, config.maximumPower);
+
+        if (Math.abs(constrainedCorrection) < config.deadBand)
+            return 0;
+        return constrainedCorrection;
+
     }
 
     public static final class MaplePIDConfig {
-        final double maximumPower, errorStartDecelerate, deadBand, errorTolerance, timeThinkAhead;
-        final double Kp, Ki, Kd;
-        final boolean isCircularLoop;
+        public final double maximumPower, errorStartDecelerate, deadBand, errorTolerance, timeThinkAhead;
+        public final double Kp, Ki, Kd;
+        public final boolean isCircularLoop;
 
         public MaplePIDConfig(double maximumPower, double errorStartDecelerate, double percentDeadBand, double errorTolerance, double timeThinkAhead, boolean isCircularLoop, double ki) {
             this.maximumPower = maximumPower;