From b07d9e1ff74a0d2385a7b4af6dc54295f3149d07 Mon Sep 17 00:00:00 2001 From: Geoff Schmit Date: Sat, 7 Oct 2023 16:19:04 -0500 Subject: [PATCH] Characterization fix (#54) * fix #53; don't use drive method or driveMode * update constants after characterization * update ramp rate based on length of carpet --- .../java/frc/lib/team3061/RobotConfig.java | 3 +- .../commands/FeedForwardCharacterization.java | 2 +- .../frc/robot/configs/NovaRobotConfig.java | 12 +++---- .../subsystems/drivetrain/Drivetrain.java | 36 ++++--------------- 4 files changed, 15 insertions(+), 38 deletions(-) diff --git a/src/main/java/frc/lib/team3061/RobotConfig.java b/src/main/java/frc/lib/team3061/RobotConfig.java index 0a2bfeb7..3f1aca55 100644 --- 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 index a776a2b6..81632cf3 100644 --- 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 index 8505a53d..9f1df44c 100644 --- 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 index e497d4ad..56cfbc74 100644 --- 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 } }