Characterization fix (#54)

* fix #53; don't use drive method or driveMode * update constants after characterization * update ramp rate based on length of carpet
HuskieRobotics · Oct 7, 2023 · b07d9e1 · b07d9e1
1 parent e342de8
commit b07d9e1
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 38 deletions.
diff --git a/src/main/java/frc/lib/team3061/RobotConfig.java b/src/main/java/frc/lib/team3061/RobotConfig.java
@@ -65,10 +65,9 @@ public double getSwerveAngleKD() {
     return 0.0;
   }
 
-
   /*
    * Returns the voltage needed to overcome the swerve module's static friction. Defaults to 0.
-   * 
+   *
    * @return the voltage needed to overcome the swerve module's static friction
    */
   public double getSwerveAngleKS() {

diff --git a/src/main/java/frc/robot/commands/FeedForwardCharacterization.java b/src/main/java/frc/robot/commands/FeedForwardCharacterization.java
@@ -21,7 +21,7 @@
 @java.lang.SuppressWarnings({"java:S106", "java:S107"})
 public class FeedForwardCharacterization extends CommandBase {
   private static final double START_DELAY_SECS = 2.0;
-  private static final double RAMP_RATE_VOLTS_PER_SECOND = 1.75;
+  private static final double RAMP_RATE_VOLTS_PER_SECOND = 2.0;
 
   private final boolean forwards;
   private final boolean isDrive;

diff --git a/src/main/java/frc/robot/configs/NovaRobotConfig.java b/src/main/java/frc/robot/configs/NovaRobotConfig.java
@@ -45,18 +45,18 @@ public class NovaRobotConfig extends RobotConfig {
   private static final double ANGLE_KI = 0.0;
   private static final double ANGLE_KD = 0.0;
 
-  private static final double ANGLE_KS = 0.17853;
-  private static final double ANGLE_KV = 0.43784;
-  private static final double ANGLE_KA = 0.0;
+  private static final double ANGLE_KS = 0.1891233333;
+  private static final double ANGLE_KV = 0.4399866667;
+  private static final double ANGLE_KA = 0.001663333333;
 
   /* Drive Motor PID Values */
   private static final double DRIVE_KP = 0.2402346041055719;
   private static final double DRIVE_KI = 0.0;
   private static final double DRIVE_KD = 0.013212903225806451;
 
-  private static final double DRIVE_KS = 0.37182;
-  private static final double DRIVE_KV = 2.72624;
-  private static final double DRIVE_KA = 0.0;
+  private static final double DRIVE_KS = 0.4004375;
+  private static final double DRIVE_KV = 2.7637325;
+  private static final double DRIVE_KA = 0.0139575;
 
   private static final SwerveType SWERVE_TYPE = SwerveType.MK4I;
 

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -131,7 +131,6 @@ public class Drivetrain extends SubsystemBase {
   private static final double BREAK_MODE_DELAY_SEC = 10.0;
 
   private DriveMode driveMode = DriveMode.NORMAL;
-  private double characterizationVoltage = 0.0;
 
   private boolean isTurbo;
 
@@ -449,21 +448,6 @@ public void drive(
         setSwerveModuleStates(newSwerveModuleStates, isOpenLoop, false);
         break;
 
-      case SWERVE_DRIVE_CHARACTERIZATION:
-        // In this characterization mode, drive at the specified voltage (and turn to zero degrees)
-        for (SwerveModule swerveModule : swerveModules) {
-          swerveModule.setVoltageForDriveCharacterization(characterizationVoltage);
-        }
-        break;
-
-      case SWERVE_ROTATE_CHARACTERIZATION:
-        // In this characterization mode, rotate the swerve modules at the specified voltage (and
-        // don't drive)
-        for (SwerveModule swerveModule : swerveModules) {
-          swerveModule.setVoltageForRotateCharacterization(characterizationVoltage);
-        }
-        break;
-
       case X:
         this.setXStance();
         break;
@@ -762,11 +746,9 @@ public PIDController getAutoThetaController() {
    * @param volts the commanded voltage
    */
   public void runDriveCharacterizationVolts(double volts) {
-    driveMode = DriveMode.SWERVE_DRIVE_CHARACTERIZATION;
-    characterizationVoltage = volts;
-
-    // invoke drive which will set the characterization voltage to each module
-    drive(0, 0, 0, true, false);
+    for (SwerveModule swerveModule : swerveModules) {
+      swerveModule.setVoltageForDriveCharacterization(volts);
+    }
   }
 
   /**
@@ -799,11 +781,9 @@ public double getDriveCharacterizationAcceleration() {
    * @param volts the commanded voltage
    */
   public void runRotateCharacterizationVolts(double volts) {
-    driveMode = DriveMode.SWERVE_ROTATE_CHARACTERIZATION;
-    characterizationVoltage = volts;
-
-    // invoke drive which will set the characterization voltage to each module
-    drive(0, 0, 0, true, false);
+    for (SwerveModule swerveModule : swerveModules) {
+      swerveModule.setVoltageForRotateCharacterization(volts);
+    }
   }
 
   /**
@@ -919,8 +899,6 @@ private static ChassisSpeeds convertFromDiscreteChassisSpeedsToContinuous(
 
   private enum DriveMode {
     NORMAL,
-    X,
-    SWERVE_DRIVE_CHARACTERIZATION,
-    SWERVE_ROTATE_CHARACTERIZATION
+    X
   }
 }