drive tuning

Author: ds12a

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

@@ -32,7 +32,7 @@ public class AlgaeConstants {
   public static final double kIntakingSpeed = -1;
 
   public static final double kHasAlgaeVelThreshold = 10;
-  public static final double kHasAlgaeCounts = 3;
+  public static final double kHasAlgaeCounts = 2;
 
   // Example Talon FX Config
   public static TalonFXSConfiguration getFXConfig() {

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

@@ -166,7 +166,8 @@ public static TalonFXConfiguration getDriveTalonConfig() {
 
     CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs();
     currentConfig.SupplyCurrentLimit = 60;
-
+    currentConfig.SupplyCurrentLowerLimit = 60;
+    currentConfig.SupplyCurrentLowerTime = 1.0;
     currentConfig.StatorCurrentLimit = 140;
 
     currentConfig.SupplyCurrentLimitEnable = true;
@@ -175,10 +176,13 @@ public static TalonFXConfiguration getDriveTalonConfig() {
     driveConfig.CurrentLimits = currentConfig;
 
     Slot0Configs slot0Config = new Slot0Configs();
-    slot0Config.kP = 0.5; // 0.16 using phoenix 6 migrate
-    slot0Config.kI = 0.5; // 0.0002 using phoenix 6 migrate
+    slot0Config.kP = 0.2;
+    slot0Config.kI = 0.0;
     slot0Config.kD = 0.0;
-    slot0Config.kV = 0.12; // 0.047 using phoenix 6 migrate
+    slot0Config.kV = 0.117;
+    slot0Config.kS = 0.0;
+    slot0Config.kA = 0.0;
+    slot0Config.kG = 0.0;
     driveConfig.Slot0 = slot0Config;
 
     MotorOutputConfigs motorConfigs = new MotorOutputConfigs();

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

@@ -54,7 +54,7 @@ public class TagServoingConstants {
   public static final double kAlgaeStopXDriveRadius =
       kAlgaeInitialDriveRadius; // Should be closer to reef than target pose
   public static final double kCoralDriveCloseEnough = 0.1;
-  public static final double kAlgaeDriveCloseEnough = 0.2;
+  public static final double kAlgaeDriveCloseEnough = 0.1;
   public static final double kMinVelX = 0.85;
 
   // Reef

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

@@ -161,15 +161,7 @@ private ChassisSpeeds getRobotRelSpeed() {
     return kinematics.toChassisSpeeds(swerveModuleStates);
   }
 
-  private ChassisSpeeds getFieldRelSpeed() {
-    // SwerveDriveKinematics kinematics = swerveDrive.getKinematics();
-    // SwerveModule[] swerveModules = swerveDrive.getSwerveModules();
-    SwerveModuleState[] swerveModuleStates = new SwerveModuleState[4];
-    for (int i = 0; i < 4; ++i) {
-      swerveModuleStates[i] = swerveModules[i].getState();
-    }
-    ChassisSpeeds roboRelSpeed = kinematics.toChassisSpeeds(swerveModuleStates);
-
+  private ChassisSpeeds getFieldRelSpeed(ChassisSpeeds roboRelSpeed) {
     Rotation2d heading = swerveDrive.getHeading().unaryMinus();
     fieldX =
         roboRelSpeed.vxMetersPerSecond * heading.getCos()
@@ -268,7 +260,8 @@ public void updateInputs(SwerveIOInputs inputs) {
       inputs.azimuthVels[i] = azimuths[i].getSelectedSensorVelocity();
       inputs.azimuthCurrent[i] = azimuths[i].getSupplyCurrent();
     }
-    inputs.fieldRelSpeed = getFieldRelSpeed();
+    inputs.robotRelSpeed = getRobotRelSpeed();
+    inputs.fieldRelSpeed = getFieldRelSpeed(inputs.robotRelSpeed);
     inputs.fieldY = fieldY;
     inputs.fieldX = fieldX;
   }

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

@@ -35,6 +35,7 @@ public static class SwerveIOInputs {
     public double pigeonTemp = 0;
     public double fieldX = 0;
     public double fieldY = 0;
+    public ChassisSpeeds robotRelSpeed = new ChassisSpeeds();
     public ChassisSpeeds fieldRelSpeed = new ChassisSpeeds();
     public double[] azimuthVels = {0, 0, 0, 0};
     public double[] azimuthCurrent = {0, 0, 0, 0};

src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java

@@ -62,7 +62,7 @@ public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSu
         new ProfiledPIDController(0.0017, 0, 0, TagServoingConstants.alignXConstraints); // 0.0015
     this.alignY = new ProfiledPIDController(0.00195, 0, 0, TagServoingConstants.alignYConstraints);
     this.alignOmega =
-        new ProfiledPIDController(6, 0, 0, TagServoingConstants.alignOmegaConstraints);
+        new ProfiledPIDController(5.5, 0, 0, TagServoingConstants.alignOmegaConstraints);
     this.alignOmega.enableContinuousInput(Math.toRadians(-180), Math.toRadians(180));
 
     Logger.recordOutput("TagAlignSubsystem/TargetDiag", -1);
@@ -357,8 +357,8 @@ public void periodic() {
         double vX = -alignX.calculate(goalTargetDiag - diag, 0);
         double vY = -alignY.calculate(TagServoingConstants.kHorizontalTarget - center.x(), 0);
 
-        Logger.recordOutput("TagAlignSubsystem/Tag Align X Error", alignX.getPositionError());
-        Logger.recordOutput("TagAlignSubsystem/Tag Align Y Error", alignY.getPositionError());
+        Logger.recordOutput("TagAlignSubsystem/Diag Error", alignX.getPositionError());
+        Logger.recordOutput("TagAlignSubsystem/Horizontal Error", alignY.getPositionError());
         Logger.recordOutput(
             "TagAlignSubsystem/Last result delay",
             (RobotController.getFPGATime() - result.getTimeStamp()) / 1000);