From 0ebd3bfe67341bd94d7863f2c9dbfb92f2d015f8 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sat, 22 Feb 2025 10:44:37 -0500 Subject: [PATCH 01/20] contants 2024 --- .../java/frc/robot/constants/Constants.java | 2 +- .../robot/constants/ConstantsChassis2024.java | 24 +++++++++++++++++++ .../frc/robot/constants/GameConstants.java | 2 +- .../io/drive/SparkMaxDriveMotorIO.java | 1 - 4 files changed, 26 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/constants/ConstantsChassis2024.java diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index c9b22d24..591250e4 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -1,3 +1,3 @@ package frc.robot.constants; -public class Constants extends Constants2025 {} +public class Constants extends ConstantsChassis2024 {} diff --git a/src/main/java/frc/robot/constants/ConstantsChassis2024.java b/src/main/java/frc/robot/constants/ConstantsChassis2024.java new file mode 100644 index 00000000..9739c8d5 --- /dev/null +++ b/src/main/java/frc/robot/constants/ConstantsChassis2024.java @@ -0,0 +1,24 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.constants; + +/** Add your docs here. */ +public class ConstantsChassis2024 extends Constants2025{ +// Drive +public static final int DRIVE_FRONT_RIGHT_D = 59; +public static final int DRIVE_BACK_RIGHT_D = 60; +public static final int DRIVE_FRONT_LEFT_D = 58; +public static final int DRIVE_BACK_LEFT_D = 57; +public static final int DRIVE_CANCODER_FRONT_RIGHT = 39; +public static final int DRIVE_CANCODER_BACK_RIGHT = 40; +public static final int DRIVE_CANCODER_FRONT_LEFT = 38; +public static final int DRIVE_CANCODER_BACK_LEFT = 37; + +// Steer +public static final int DRIVE_FRONT_RIGHT_S = 29; +public static final int DRIVE_BACK_RIGHT_S = 30; +public static final int DRIVE_FRONT_LEFT_S = 28; +public static final int DRIVE_BACK_LEFT_S = 27; +} diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index eceaaa95..55787d2b 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -91,7 +91,7 @@ public enum Mode { } // Limits - public static final int DRIVE_SMART_LIMIT = 38; // TODO: change later + public static final int DRIVE_SMART_LIMIT = 50; // TODO: change later public static final int DRIVE_SECONDARY_LIMIT = 48; // TODO: change later public static final double DRIVE_RAMP_RATE_LIMIT = 0.1; // TODO: change later public static final int NEO_CURRENT_LIMIT = 20; diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java index ca204a8d..20963fed 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java @@ -48,7 +48,6 @@ private void setConversionFactors(KinematicsConversionConfig conversionConfig) { } private void setMotorConfig(boolean driveInverted) { - // driveMotor.restoreFactoryDefaults(); //TODO: idk what to do driveConfig .inverted(driveInverted) .idleMode(IdleMode.kBrake) From b85ac3e4593f14271b806e5510b95fa77c6c474e Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sat, 22 Feb 2025 11:27:05 -0500 Subject: [PATCH 02/20] update --- .../robot/constants/ConstantsChassis2024.java | 35 +++++++++++-------- 1 file changed, 20 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/constants/ConstantsChassis2024.java b/src/main/java/frc/robot/constants/ConstantsChassis2024.java index 9739c8d5..b23a0128 100644 --- a/src/main/java/frc/robot/constants/ConstantsChassis2024.java +++ b/src/main/java/frc/robot/constants/ConstantsChassis2024.java @@ -5,20 +5,25 @@ package frc.robot.constants; /** Add your docs here. */ -public class ConstantsChassis2024 extends Constants2025{ -// Drive -public static final int DRIVE_FRONT_RIGHT_D = 59; -public static final int DRIVE_BACK_RIGHT_D = 60; -public static final int DRIVE_FRONT_LEFT_D = 58; -public static final int DRIVE_BACK_LEFT_D = 57; -public static final int DRIVE_CANCODER_FRONT_RIGHT = 39; -public static final int DRIVE_CANCODER_BACK_RIGHT = 40; -public static final int DRIVE_CANCODER_FRONT_LEFT = 38; -public static final int DRIVE_CANCODER_BACK_LEFT = 37; +public class ConstantsChassis2024 extends Constants2025 { + // Drive + public static final int DRIVE_FRONT_RIGHT_D = 59; + public static final int DRIVE_BACK_RIGHT_D = 60; + public static final int DRIVE_FRONT_LEFT_D = 58; + public static final int DRIVE_BACK_LEFT_D = 57; + public static final int DRIVE_CANCODER_FRONT_RIGHT = 39; + public static final int DRIVE_CANCODER_BACK_RIGHT = 40; + public static final int DRIVE_CANCODER_FRONT_LEFT = 38; + public static final int DRIVE_CANCODER_BACK_LEFT = 37; -// Steer -public static final int DRIVE_FRONT_RIGHT_S = 29; -public static final int DRIVE_BACK_RIGHT_S = 30; -public static final int DRIVE_FRONT_LEFT_S = 28; -public static final int DRIVE_BACK_LEFT_S = 27; + // Steer + public static final int DRIVE_FRONT_RIGHT_S = 29; + public static final int DRIVE_BACK_RIGHT_S = 30; + public static final int DRIVE_FRONT_LEFT_S = 28; + public static final int DRIVE_BACK_LEFT_S = 27; + + public static final double BACK_RIGHT_ABS_ENCODER_ZERO = 261.56; + public static final double FRONT_LEFT_ABS_ENCODER_ZERO = 190.28; + public static final double BACK_LEFT_ABS_ENCODER_ZERO = 78.22; + public static final double FRONT_RIGHT_ABS_ENCODER_ZERO = 306.29; } From fd1ea36c5c53d247966596def202b8a83d02eb0b Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sat, 22 Feb 2025 16:53:12 -0500 Subject: [PATCH 03/20] update --- .../java/frc/robot/constants/ConstantsChassis2024.java | 8 ++++---- src/main/java/frc/robot/constants/GameConstants.java | 6 ++++++ .../swervev3/io/drive/SparkMaxDriveMotorIO.java | 2 ++ 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/constants/ConstantsChassis2024.java b/src/main/java/frc/robot/constants/ConstantsChassis2024.java index b23a0128..d28fbe85 100644 --- a/src/main/java/frc/robot/constants/ConstantsChassis2024.java +++ b/src/main/java/frc/robot/constants/ConstantsChassis2024.java @@ -22,8 +22,8 @@ public class ConstantsChassis2024 extends Constants2025 { public static final int DRIVE_FRONT_LEFT_S = 28; public static final int DRIVE_BACK_LEFT_S = 27; - public static final double BACK_RIGHT_ABS_ENCODER_ZERO = 261.56; - public static final double FRONT_LEFT_ABS_ENCODER_ZERO = 190.28; - public static final double BACK_LEFT_ABS_ENCODER_ZERO = 78.22; - public static final double FRONT_RIGHT_ABS_ENCODER_ZERO = 306.29; + public static final double BACK_RIGHT_ABS_ENCODER_ZERO = .479; + public static final double FRONT_LEFT_ABS_ENCODER_ZERO = -.215; + public static final double BACK_LEFT_ABS_ENCODER_ZERO = .470; + public static final double FRONT_RIGHT_ABS_ENCODER_ZERO = -.400; } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 55787d2b..e5c1aa2b 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -108,6 +108,7 @@ public enum Mode { public static final double HIHI_PID_ALLOWED_ERROR = 0.5; public static final boolean HIHI_USE_MAX_MOTION = false; + // Elevator PID public static final boolean ELEVATOR_USE_MAX_MOTION = true; @@ -117,6 +118,11 @@ public enum Mode { public static final double DRIVE_PID_D = 0; // TODO: change later public static final double DRIVE_PID_FF_S = 1; // TODO: change later public static final double DRIVE_PID_FF_V = 2.8; // TODO: change later + public static final double DRIVE_PID_I_ZONE = 0; // TODO: change later + public static final double DRIVE_PID_MAX_VEL = 0; + public static final double DRIVE_PID_MAX_ACC = 0; + public static final double DRIVE_PID_ALLOWED_ERROR = 0; + public static final int DRIVE_CURRENT_LIMIT = 0; // Steer PID public static final double STEER_PID_P = 0.3; // TODO: change later diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java index 20963fed..f45c2894 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java @@ -9,6 +9,8 @@ import frc.robot.subsystems.swervev3.KinematicsConversionConfig; import frc.robot.utils.logging.subsystem.inputs.MotorInputs; import frc.robot.utils.logging.subsystem.providers.SparkMaxInputProvider; +import frc.robot.utils.motor.NeoPidConfig; +import frc.robot.utils.motor.TunablePIDManager; public class SparkMaxDriveMotorIO implements SwerveDriveMotorIO { From e904366383f62c18a300a7f56022da00c3f7e722 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sat, 22 Feb 2025 17:44:35 -0500 Subject: [PATCH 04/20] added Current limiting tunable# --- .../frc/robot/constants/GameConstants.java | 1 - .../subsystems/swervev3/SwerveDrivetrain.java | 25 ++++++++++++++++++- .../subsystems/swervev3/io/SwerveModule.java | 3 +++ .../swervev3/io/drive/MockDriveMotorIO.java | 3 +++ .../io/drive/SparkMaxDriveMotorIO.java | 16 +++++++++--- .../swervev3/io/drive/SwerveDriveMotorIO.java | 2 ++ 6 files changed, 45 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index e5c1aa2b..2682df30 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -122,7 +122,6 @@ public enum Mode { public static final double DRIVE_PID_MAX_VEL = 0; public static final double DRIVE_PID_MAX_ACC = 0; public static final double DRIVE_PID_ALLOWED_ERROR = 0; - public static final int DRIVE_CURRENT_LIMIT = 0; // Steer PID public static final double STEER_PID_P = 0.3; // TODO: change later diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index c0cb91e5..5101833c 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -17,6 +17,7 @@ import frc.robot.subsystems.swervev3.io.SwerveModule; import frc.robot.utils.DriveMode; import frc.robot.utils.logging.LoggableIO; +import frc.robot.utils.logging.LoggedTunableNumber; import frc.robot.utils.logging.subsystem.LoggableSystem; import frc.robot.utils.shuffleboard.SmartShuffleboard; import org.littletonrobotics.junction.Logger; @@ -41,7 +42,9 @@ public class SwerveDrivetrain extends SubsystemBase { private DriveMode driveMode = DriveMode.FIELD_CENTRIC; private final PoseEstimator poseEstimator; private boolean facingTarget = false; - + private LoggedTunableNumber closedLoopTunable; + private LoggedTunableNumber smartLimitTunable; + private LoggedTunableNumber secondaryLimitTunable; public SwerveDrivetrain( SwerveModule frontLeftModule, SwerveModule frontRightModule, @@ -57,6 +60,9 @@ public SwerveDrivetrain( this.poseEstimator = new PoseEstimator( frontLeft, frontRight, backLeft, backRight, apriltagIO, kinematics, getLastGyro()); + closedLoopTunable = new LoggedTunableNumber("Swerve/ClosedLoop", Constants.DRIVE_RAMP_RATE_LIMIT); + smartLimitTunable = new LoggedTunableNumber("Swerve/SmartLimit", Constants.DRIVE_SMART_LIMIT); + secondaryLimitTunable = new LoggedTunableNumber("Swerve/SecondaryLimit", Constants.DRIVE_SECONDARY_LIMIT); } @Override @@ -87,6 +93,10 @@ public void periodic() { SmartShuffleboard.put("Drive", "BL ABS Pos", backLeft.getAbsPosition()); SmartShuffleboard.put("Drive", "BR ABS Pos", backRight.getAbsPosition()); } + LoggedTunableNumber.ifChanged( + hashCode(), () -> { + updateConfig(closedLoopTunable.get(),secondaryLimitTunable.get(),(int)smartLimitTunable.get() ); + }, closedLoopTunable, secondaryLimitTunable, smartLimitTunable); } private void processInputs() { @@ -204,4 +214,17 @@ public void setFacingTarget(boolean facingTarget) { public boolean isFacingTarget() { return facingTarget; } + /** + * + * @param closedLoopRampRate + * @param secondaryCurrentLimit + * @param smartCurrentLimit + */ + public void updateConfig(double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) { + frontLeft.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); + frontRight.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); + backRight.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); + backLeft.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); + } } + diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java index 53f10d51..b95b7459 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java @@ -155,4 +155,7 @@ private double getSteerPosition() { return AngleUtils.normalizeSwerveAngle( steerSystem.getInputs().getEncoderPosition() - steerOffset); } + public void updateConfig(double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit ){ + driveSystem.getIO().updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); + } } diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/drive/MockDriveMotorIO.java b/src/main/java/frc/robot/subsystems/swervev3/io/drive/MockDriveMotorIO.java index b0032e0d..fac7dd28 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/drive/MockDriveMotorIO.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/drive/MockDriveMotorIO.java @@ -12,4 +12,7 @@ public void resetEncoder() {} @Override public void updateInputs(MotorInputs inputs) {} + + @Override + public void updateConfig(double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) {} } diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java index f45c2894..0671381a 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java @@ -9,13 +9,11 @@ import frc.robot.subsystems.swervev3.KinematicsConversionConfig; import frc.robot.utils.logging.subsystem.inputs.MotorInputs; import frc.robot.utils.logging.subsystem.providers.SparkMaxInputProvider; -import frc.robot.utils.motor.NeoPidConfig; -import frc.robot.utils.motor.TunablePIDManager; public class SparkMaxDriveMotorIO implements SwerveDriveMotorIO { private final SparkMax driveMotor; - private final SparkBaseConfig driveConfig; + private final SparkMaxConfig driveConfig; private final SparkMaxInputProvider inputProvider; public SparkMaxDriveMotorIO( @@ -72,4 +70,16 @@ public void resetEncoder() { public void updateInputs(MotorInputs inputs) { inputs.process(inputProvider); } + + @Override + public void updateConfig(double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) { + driveConfig + .closedLoopRampRate(closedLoopRampRate) + .secondaryCurrentLimit(secondaryCurrentLimit) + .smartCurrentLimit(smartCurrentLimit); + driveMotor.configure( + driveConfig, + SparkBase.ResetMode.kNoResetSafeParameters, + SparkBase.PersistMode.kNoPersistParameters); + } } diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java index 4e5ddb75..4b5b3da9 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java @@ -7,4 +7,6 @@ public interface SwerveDriveMotorIO extends LoggableIO { void setDriveVoltage(double volts); void resetEncoder(); + + public void updateConfig(double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit); } From 2a57573a64a7244626681d8748ba0b8be983caae Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sat, 22 Feb 2025 18:31:30 -0500 Subject: [PATCH 05/20] added a bunch of stuff --- .../frc/robot/constants/GameConstants.java | 3 +- .../subsystems/swervev3/SwerveDrivetrain.java | 62 ++++++++++++++++--- .../subsystems/swervev3/io/SwerveModule.java | 11 +++- .../swervev3/io/drive/MockDriveMotorIO.java | 3 +- .../io/drive/SparkMaxDriveMotorIO.java | 12 ++-- .../swervev3/io/drive/SwerveDriveMotorIO.java | 3 +- 6 files changed, 73 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 2682df30..3706c31c 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -108,7 +108,6 @@ public enum Mode { public static final double HIHI_PID_ALLOWED_ERROR = 0.5; public static final boolean HIHI_USE_MAX_MOTION = false; - // Elevator PID public static final boolean ELEVATOR_USE_MAX_MOTION = true; @@ -121,7 +120,7 @@ public enum Mode { public static final double DRIVE_PID_I_ZONE = 0; // TODO: change later public static final double DRIVE_PID_MAX_VEL = 0; public static final double DRIVE_PID_MAX_ACC = 0; - public static final double DRIVE_PID_ALLOWED_ERROR = 0; + public static final double DRIVE_PID_ALLOWED_ERROR = 0; // Steer PID public static final double STEER_PID_P = 0.3; // TODO: change later diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 5101833c..ecbb1bc3 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -45,6 +45,13 @@ public class SwerveDrivetrain extends SubsystemBase { private LoggedTunableNumber closedLoopTunable; private LoggedTunableNumber smartLimitTunable; private LoggedTunableNumber secondaryLimitTunable; + private LoggedTunableNumber drivePTunable; + private LoggedTunableNumber driveITunable; + private LoggedTunableNumber driveDTunable; + private LoggedTunableNumber steerPTunable; + private LoggedTunableNumber steerITunable; + private LoggedTunableNumber steerDTunable; + public SwerveDrivetrain( SwerveModule frontLeftModule, SwerveModule frontRightModule, @@ -60,9 +67,17 @@ public SwerveDrivetrain( this.poseEstimator = new PoseEstimator( frontLeft, frontRight, backLeft, backRight, apriltagIO, kinematics, getLastGyro()); - closedLoopTunable = new LoggedTunableNumber("Swerve/ClosedLoop", Constants.DRIVE_RAMP_RATE_LIMIT); + closedLoopTunable = + new LoggedTunableNumber("Swerve/ClosedLoop", Constants.DRIVE_RAMP_RATE_LIMIT); smartLimitTunable = new LoggedTunableNumber("Swerve/SmartLimit", Constants.DRIVE_SMART_LIMIT); - secondaryLimitTunable = new LoggedTunableNumber("Swerve/SecondaryLimit", Constants.DRIVE_SECONDARY_LIMIT); + secondaryLimitTunable = + new LoggedTunableNumber("Swerve/SecondaryLimit", Constants.DRIVE_SECONDARY_LIMIT); + drivePTunable = new LoggedTunableNumber("Swerve/driveP", Constants.DRIVE_PID_P); + driveITunable = new LoggedTunableNumber("Swerve/driveI", Constants.DRIVE_PID_I); + driveDTunable = new LoggedTunableNumber("Swerve/driveD", Constants.DRIVE_PID_D); + steerPTunable = new LoggedTunableNumber("Swerve/steerP", Constants.STEER_PID_P); + steerITunable = new LoggedTunableNumber("Swerve/steerI", Constants.STEER_PID_I); + steerDTunable = new LoggedTunableNumber("Swerve/steerD", Constants.STEER_PID_D); } @Override @@ -94,10 +109,37 @@ public void periodic() { SmartShuffleboard.put("Drive", "BR ABS Pos", backRight.getAbsPosition()); } LoggedTunableNumber.ifChanged( - hashCode(), () -> { - updateConfig(closedLoopTunable.get(),secondaryLimitTunable.get(),(int)smartLimitTunable.get() ); - }, closedLoopTunable, secondaryLimitTunable, smartLimitTunable); - } + hashCode(), + () -> { + updateConfig( + closedLoopTunable.get(), secondaryLimitTunable.get(), (int) smartLimitTunable.get()); + }, + closedLoopTunable, + secondaryLimitTunable, + smartLimitTunable); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + frontLeft.setDrivePID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); + frontRight.setDrivePID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); + backLeft.setDrivePID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); + backRight.setDrivePID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); + }, + drivePTunable, + driveITunable, + driveDTunable); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + frontLeft.setSteerPID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); + frontRight.setSteerPID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); + backLeft.setSteerPID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); + backRight.setSteerPID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); + }, + steerPTunable, + steerITunable, + steerDTunable); + } private void processInputs() { frontLeft.updateInputs(); @@ -214,17 +256,17 @@ public void setFacingTarget(boolean facingTarget) { public boolean isFacingTarget() { return facingTarget; } + /** - * * @param closedLoopRampRate * @param secondaryCurrentLimit * @param smartCurrentLimit */ - public void updateConfig(double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) { + public void updateConfig( + double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) { frontLeft.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); frontRight.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); backRight.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); - backLeft.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); + backLeft.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); } } - diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java index b95b7459..02228b0f 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java @@ -155,7 +155,16 @@ private double getSteerPosition() { return AngleUtils.normalizeSwerveAngle( steerSystem.getInputs().getEncoderPosition() - steerOffset); } - public void updateConfig(double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit ){ + + public void setDrivePID(double P, double I, double D) { + drivePIDController.setPID(P, I, D); + } + public void setSteerPID(double P, double I, double D) { + turningPIDController.setPID(P, I, D); + } + + public void updateConfig( + double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) { driveSystem.getIO().updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); } } diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/drive/MockDriveMotorIO.java b/src/main/java/frc/robot/subsystems/swervev3/io/drive/MockDriveMotorIO.java index fac7dd28..ebb7c304 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/drive/MockDriveMotorIO.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/drive/MockDriveMotorIO.java @@ -14,5 +14,6 @@ public void resetEncoder() {} public void updateInputs(MotorInputs inputs) {} @Override - public void updateConfig(double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) {} + public void updateConfig( + double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) {} } diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java index 0671381a..6ab179f0 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java @@ -2,7 +2,6 @@ import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; import frc.robot.constants.Constants; @@ -72,13 +71,14 @@ public void updateInputs(MotorInputs inputs) { } @Override - public void updateConfig(double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) { + public void updateConfig( + double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) { driveConfig - .closedLoopRampRate(closedLoopRampRate) - .secondaryCurrentLimit(secondaryCurrentLimit) - .smartCurrentLimit(smartCurrentLimit); + .closedLoopRampRate(closedLoopRampRate) + .secondaryCurrentLimit(secondaryCurrentLimit) + .smartCurrentLimit(smartCurrentLimit); driveMotor.configure( - driveConfig, + driveConfig, SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters); } diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java index 4b5b3da9..9d034d77 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java @@ -8,5 +8,6 @@ public interface SwerveDriveMotorIO extends LoggableIO { void resetEncoder(); - public void updateConfig(double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit); + public void updateConfig( + double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit); } From 686f5dc0692db8a79999e5376ddfde93a768db9e Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sat, 22 Feb 2025 18:57:10 -0500 Subject: [PATCH 06/20] added steer motor tunnables --- .../subsystems/swervev3/SwerveDrivetrain.java | 28 +++++++++++-------- .../subsystems/swervev3/io/SwerveModule.java | 4 ++- 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index ecbb1bc3..3956e16e 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -51,6 +51,8 @@ public class SwerveDrivetrain extends SubsystemBase { private LoggedTunableNumber steerPTunable; private LoggedTunableNumber steerITunable; private LoggedTunableNumber steerDTunable; + private LoggedTunableNumber steerMaxAccelerationTunable; + private LoggedTunableNumber steerMaxVelocityTunable; public SwerveDrivetrain( SwerveModule frontLeftModule, @@ -72,12 +74,14 @@ public SwerveDrivetrain( smartLimitTunable = new LoggedTunableNumber("Swerve/SmartLimit", Constants.DRIVE_SMART_LIMIT); secondaryLimitTunable = new LoggedTunableNumber("Swerve/SecondaryLimit", Constants.DRIVE_SECONDARY_LIMIT); - drivePTunable = new LoggedTunableNumber("Swerve/driveP", Constants.DRIVE_PID_P); - driveITunable = new LoggedTunableNumber("Swerve/driveI", Constants.DRIVE_PID_I); - driveDTunable = new LoggedTunableNumber("Swerve/driveD", Constants.DRIVE_PID_D); - steerPTunable = new LoggedTunableNumber("Swerve/steerP", Constants.STEER_PID_P); - steerITunable = new LoggedTunableNumber("Swerve/steerI", Constants.STEER_PID_I); - steerDTunable = new LoggedTunableNumber("Swerve/steerD", Constants.STEER_PID_D); + drivePTunable = new LoggedTunableNumber("Swerve/drive/P", Constants.DRIVE_PID_P); + driveITunable = new LoggedTunableNumber("Swerve/drive/I", Constants.DRIVE_PID_I); + driveDTunable = new LoggedTunableNumber("Swerve/drive/D", Constants.DRIVE_PID_D); + steerPTunable = new LoggedTunableNumber("Swerve/steer/P", Constants.STEER_PID_P); + steerITunable = new LoggedTunableNumber("Swerve/steer/I", Constants.STEER_PID_I); + steerDTunable = new LoggedTunableNumber("Swerve/steer/D", Constants.STEER_PID_D); + steerMaxAccelerationTunable = new LoggedTunableNumber("Swerve/steer/maxAccel", Constants.MAX_ANGULAR_SPEED * 150); + steerMaxVelocityTunable = new LoggedTunableNumber("Swerve/steer/maxVelocity", 2 * Math.PI * 150); } @Override @@ -131,14 +135,16 @@ public void periodic() { LoggedTunableNumber.ifChanged( hashCode(), () -> { - frontLeft.setSteerPID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); - frontRight.setSteerPID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); - backLeft.setSteerPID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); - backRight.setSteerPID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); + frontLeft.setSteerPID(steerPTunable.get(), steerITunable.get(), steerDTunable.get(), steerMaxAccelerationTunable.get(), steerMaxVelocityTunable.get()); + frontRight.setSteerPID(steerPTunable.get(), steerITunable.get(), steerDTunable.get(), steerMaxAccelerationTunable.get(), steerMaxVelocityTunable.get()); + backLeft.setSteerPID(steerPTunable.get(), steerITunable.get(), steerDTunable.get(), steerMaxAccelerationTunable.get(), steerMaxVelocityTunable.get()); + backRight.setSteerPID(steerPTunable.get(), steerITunable.get(), steerDTunable.get(), steerMaxAccelerationTunable.get(), steerMaxVelocityTunable.get()); }, steerPTunable, steerITunable, - steerDTunable); + steerDTunable, + steerMaxAccelerationTunable, + steerMaxVelocityTunable); } private void processInputs() { diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java index 02228b0f..527621b8 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import frc.robot.constants.Constants; import frc.robot.subsystems.swervev3.KinematicsConversionConfig; import frc.robot.subsystems.swervev3.SwerveIdConfig; import frc.robot.subsystems.swervev3.SwervePidConfig; @@ -159,8 +160,9 @@ private double getSteerPosition() { public void setDrivePID(double P, double I, double D) { drivePIDController.setPID(P, I, D); } - public void setSteerPID(double P, double I, double D) { + public void setSteerPID(double P, double I, double D, double maxAcceleration, double maxVelocity) { turningPIDController.setPID(P, I, D); + turningPIDController.setConstraints(new TrapezoidProfile.Constraints(maxAcceleration, maxVelocity)); } public void updateConfig( From 8d44ae56cd0850f5817daa062fc55273b1dd8120 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sat, 22 Feb 2025 18:57:51 -0500 Subject: [PATCH 07/20] lint --- .../subsystems/swervev3/SwerveDrivetrain.java | 54 +++++++++++++------ .../subsystems/swervev3/io/SwerveModule.java | 8 +-- 2 files changed, 43 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 3956e16e..7cd42ecf 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -80,8 +80,10 @@ public SwerveDrivetrain( steerPTunable = new LoggedTunableNumber("Swerve/steer/P", Constants.STEER_PID_P); steerITunable = new LoggedTunableNumber("Swerve/steer/I", Constants.STEER_PID_I); steerDTunable = new LoggedTunableNumber("Swerve/steer/D", Constants.STEER_PID_D); - steerMaxAccelerationTunable = new LoggedTunableNumber("Swerve/steer/maxAccel", Constants.MAX_ANGULAR_SPEED * 150); - steerMaxVelocityTunable = new LoggedTunableNumber("Swerve/steer/maxVelocity", 2 * Math.PI * 150); + steerMaxAccelerationTunable = + new LoggedTunableNumber("Swerve/steer/maxAccel", Constants.MAX_ANGULAR_SPEED * 150); + steerMaxVelocityTunable = + new LoggedTunableNumber("Swerve/steer/maxVelocity", 2 * Math.PI * 150); } @Override @@ -132,20 +134,40 @@ public void periodic() { drivePTunable, driveITunable, driveDTunable); - LoggedTunableNumber.ifChanged( - hashCode(), - () -> { - frontLeft.setSteerPID(steerPTunable.get(), steerITunable.get(), steerDTunable.get(), steerMaxAccelerationTunable.get(), steerMaxVelocityTunable.get()); - frontRight.setSteerPID(steerPTunable.get(), steerITunable.get(), steerDTunable.get(), steerMaxAccelerationTunable.get(), steerMaxVelocityTunable.get()); - backLeft.setSteerPID(steerPTunable.get(), steerITunable.get(), steerDTunable.get(), steerMaxAccelerationTunable.get(), steerMaxVelocityTunable.get()); - backRight.setSteerPID(steerPTunable.get(), steerITunable.get(), steerDTunable.get(), steerMaxAccelerationTunable.get(), steerMaxVelocityTunable.get()); - }, - steerPTunable, - steerITunable, - steerDTunable, - steerMaxAccelerationTunable, - steerMaxVelocityTunable); - } + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + frontLeft.setSteerPID( + steerPTunable.get(), + steerITunable.get(), + steerDTunable.get(), + steerMaxAccelerationTunable.get(), + steerMaxVelocityTunable.get()); + frontRight.setSteerPID( + steerPTunable.get(), + steerITunable.get(), + steerDTunable.get(), + steerMaxAccelerationTunable.get(), + steerMaxVelocityTunable.get()); + backLeft.setSteerPID( + steerPTunable.get(), + steerITunable.get(), + steerDTunable.get(), + steerMaxAccelerationTunable.get(), + steerMaxVelocityTunable.get()); + backRight.setSteerPID( + steerPTunable.get(), + steerITunable.get(), + steerDTunable.get(), + steerMaxAccelerationTunable.get(), + steerMaxVelocityTunable.get()); + }, + steerPTunable, + steerITunable, + steerDTunable, + steerMaxAccelerationTunable, + steerMaxVelocityTunable); + } private void processInputs() { frontLeft.updateInputs(); diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java index 527621b8..cf544751 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import frc.robot.constants.Constants; import frc.robot.subsystems.swervev3.KinematicsConversionConfig; import frc.robot.subsystems.swervev3.SwerveIdConfig; import frc.robot.subsystems.swervev3.SwervePidConfig; @@ -160,9 +159,12 @@ private double getSteerPosition() { public void setDrivePID(double P, double I, double D) { drivePIDController.setPID(P, I, D); } - public void setSteerPID(double P, double I, double D, double maxAcceleration, double maxVelocity) { + + public void setSteerPID( + double P, double I, double D, double maxAcceleration, double maxVelocity) { turningPIDController.setPID(P, I, D); - turningPIDController.setConstraints(new TrapezoidProfile.Constraints(maxAcceleration, maxVelocity)); + turningPIDController.setConstraints( + new TrapezoidProfile.Constraints(maxAcceleration, maxVelocity)); } public void updateConfig( From d92a7d7204ba99deb5e13b674f92ff705d44b8c2 Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sat, 22 Feb 2025 19:00:17 -0500 Subject: [PATCH 08/20] u --- .../frc/robot/subsystems/swervev3/SwerveDrivetrain.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 7cd42ecf..57fc2284 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -70,10 +70,10 @@ public SwerveDrivetrain( new PoseEstimator( frontLeft, frontRight, backLeft, backRight, apriltagIO, kinematics, getLastGyro()); closedLoopTunable = - new LoggedTunableNumber("Swerve/ClosedLoop", Constants.DRIVE_RAMP_RATE_LIMIT); - smartLimitTunable = new LoggedTunableNumber("Swerve/SmartLimit", Constants.DRIVE_SMART_LIMIT); + new LoggedTunableNumber("Swerve/currentLimiting/ClosedLoop", Constants.DRIVE_RAMP_RATE_LIMIT); + smartLimitTunable = new LoggedTunableNumber("Swerve/currentLimiting/SmartLimit", Constants.DRIVE_SMART_LIMIT); secondaryLimitTunable = - new LoggedTunableNumber("Swerve/SecondaryLimit", Constants.DRIVE_SECONDARY_LIMIT); + new LoggedTunableNumber("Swerve/currentLimiting/SecondaryLimit", Constants.DRIVE_SECONDARY_LIMIT); drivePTunable = new LoggedTunableNumber("Swerve/drive/P", Constants.DRIVE_PID_P); driveITunable = new LoggedTunableNumber("Swerve/drive/I", Constants.DRIVE_PID_I); driveDTunable = new LoggedTunableNumber("Swerve/drive/D", Constants.DRIVE_PID_D); From d11265d05254979b8ecc791c96b643204913d20f Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sat, 22 Feb 2025 19:03:27 -0500 Subject: [PATCH 09/20] update --- .../frc/robot/subsystems/swervev3/SwerveDrivetrain.java | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 57fc2284..bc8c083d 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -70,10 +70,13 @@ public SwerveDrivetrain( new PoseEstimator( frontLeft, frontRight, backLeft, backRight, apriltagIO, kinematics, getLastGyro()); closedLoopTunable = - new LoggedTunableNumber("Swerve/currentLimiting/ClosedLoop", Constants.DRIVE_RAMP_RATE_LIMIT); - smartLimitTunable = new LoggedTunableNumber("Swerve/currentLimiting/SmartLimit", Constants.DRIVE_SMART_LIMIT); + new LoggedTunableNumber( + "Swerve/currentLimiting/ClosedLoop", Constants.DRIVE_RAMP_RATE_LIMIT); + smartLimitTunable = + new LoggedTunableNumber("Swerve/currentLimiting/SmartLimit", Constants.DRIVE_SMART_LIMIT); secondaryLimitTunable = - new LoggedTunableNumber("Swerve/currentLimiting/SecondaryLimit", Constants.DRIVE_SECONDARY_LIMIT); + new LoggedTunableNumber( + "Swerve/currentLimiting/SecondaryLimit", Constants.DRIVE_SECONDARY_LIMIT); drivePTunable = new LoggedTunableNumber("Swerve/drive/P", Constants.DRIVE_PID_P); driveITunable = new LoggedTunableNumber("Swerve/drive/I", Constants.DRIVE_PID_I); driveDTunable = new LoggedTunableNumber("Swerve/drive/D", Constants.DRIVE_PID_D); From 6a49f90d644633a0e6558d127193f75ce107970c Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sat, 22 Feb 2025 20:15:24 -0500 Subject: [PATCH 10/20] Added swerve debug --- src/main/java/frc/robot/constants/Constants.java | 2 +- src/main/java/frc/robot/constants/GameConstants.java | 6 +++--- .../frc/robot/subsystems/swervev3/SwerveDrivetrain.java | 7 ++++++- 3 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 591250e4..c9b22d24 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -1,3 +1,3 @@ package frc.robot.constants; -public class Constants extends ConstantsChassis2024 {} +public class Constants extends Constants2025 {} diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 5070382d..7f4c1580 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -89,9 +89,9 @@ public enum Mode { } // Limits - public static final int DRIVE_SMART_LIMIT = 50; // TODO: change later - public static final int DRIVE_SECONDARY_LIMIT = 48; // TODO: change later - public static final double DRIVE_RAMP_RATE_LIMIT = 0.1; // TODO: change later + public static final int DRIVE_SMART_LIMIT = 40; // TODO: change later + public static final int DRIVE_SECONDARY_LIMIT = 0; // TODO: change later + public static final double DRIVE_RAMP_RATE_LIMIT = 0; // TODO: change later public static final int NEO_CURRENT_LIMIT = 20; public static final int HIHI_CURRENT_LIMIT = 10; diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index bc8c083d..cbc60211 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -42,6 +42,7 @@ public class SwerveDrivetrain extends SubsystemBase { private DriveMode driveMode = DriveMode.FIELD_CENTRIC; private final PoseEstimator poseEstimator; private boolean facingTarget = false; + private LoggedTunableNumber closedLoopTunable; private LoggedTunableNumber smartLimitTunable; private LoggedTunableNumber secondaryLimitTunable; @@ -69,7 +70,8 @@ public SwerveDrivetrain( this.poseEstimator = new PoseEstimator( frontLeft, frontRight, backLeft, backRight, apriltagIO, kinematics, getLastGyro()); - closedLoopTunable = + if(Constants.SWERVE_DEBUG){ + closedLoopTunable = new LoggedTunableNumber( "Swerve/currentLimiting/ClosedLoop", Constants.DRIVE_RAMP_RATE_LIMIT); smartLimitTunable = @@ -88,6 +90,7 @@ public SwerveDrivetrain( steerMaxVelocityTunable = new LoggedTunableNumber("Swerve/steer/maxVelocity", 2 * Math.PI * 150); } + } @Override public void periodic() { @@ -117,6 +120,7 @@ public void periodic() { SmartShuffleboard.put("Drive", "BL ABS Pos", backLeft.getAbsPosition()); SmartShuffleboard.put("Drive", "BR ABS Pos", backRight.getAbsPosition()); } + if(Constants.SWERVE_DEBUG){ LoggedTunableNumber.ifChanged( hashCode(), () -> { @@ -170,6 +174,7 @@ public void periodic() { steerDTunable, steerMaxAccelerationTunable, steerMaxVelocityTunable); + } } private void processInputs() { From 7c10f7649f7ad0253137115e65b78a8c355179cb Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sun, 23 Feb 2025 10:33:20 -0500 Subject: [PATCH 11/20] remvoed constantschasisi2024 --- .../robot/constants/ConstantsChassis2024.java | 29 ------------------- 1 file changed, 29 deletions(-) delete mode 100644 src/main/java/frc/robot/constants/ConstantsChassis2024.java diff --git a/src/main/java/frc/robot/constants/ConstantsChassis2024.java b/src/main/java/frc/robot/constants/ConstantsChassis2024.java deleted file mode 100644 index d28fbe85..00000000 --- a/src/main/java/frc/robot/constants/ConstantsChassis2024.java +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.constants; - -/** Add your docs here. */ -public class ConstantsChassis2024 extends Constants2025 { - // Drive - public static final int DRIVE_FRONT_RIGHT_D = 59; - public static final int DRIVE_BACK_RIGHT_D = 60; - public static final int DRIVE_FRONT_LEFT_D = 58; - public static final int DRIVE_BACK_LEFT_D = 57; - public static final int DRIVE_CANCODER_FRONT_RIGHT = 39; - public static final int DRIVE_CANCODER_BACK_RIGHT = 40; - public static final int DRIVE_CANCODER_FRONT_LEFT = 38; - public static final int DRIVE_CANCODER_BACK_LEFT = 37; - - // Steer - public static final int DRIVE_FRONT_RIGHT_S = 29; - public static final int DRIVE_BACK_RIGHT_S = 30; - public static final int DRIVE_FRONT_LEFT_S = 28; - public static final int DRIVE_BACK_LEFT_S = 27; - - public static final double BACK_RIGHT_ABS_ENCODER_ZERO = .479; - public static final double FRONT_LEFT_ABS_ENCODER_ZERO = -.215; - public static final double BACK_LEFT_ABS_ENCODER_ZERO = .470; - public static final double FRONT_RIGHT_ABS_ENCODER_ZERO = -.400; -} From 1fca4475fb5dd3af842bbaa16f1c6af5712b97fb Mon Sep 17 00:00:00 2001 From: jamthepersonj Date: Sun, 23 Feb 2025 10:39:56 -0500 Subject: [PATCH 12/20] udate --- .../robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java index 9d034d77..69cf49a9 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java @@ -8,6 +8,6 @@ public interface SwerveDriveMotorIO extends LoggableIO { void resetEncoder(); - public void updateConfig( + void updateConfig( double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit); } From d8a542ace39ae7153a4c8d4e653a627fb6930a5b Mon Sep 17 00:00:00 2001 From: codetoad Date: Tue, 25 Feb 2025 18:24:16 -0500 Subject: [PATCH 13/20] fixed comments --- .../java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java | 4 ++-- .../subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java | 5 +++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 0573aca8..1753ccc7 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -70,7 +70,7 @@ public SwerveDrivetrain( this.poseEstimator = new PoseEstimator( frontLeft, frontRight, backLeft, backRight, apriltagIO, kinematics, getLastGyro()); - if(Constants.SWERVE_DEBUG){ + if(Constants.TUNING_MODE){ closedLoopTunable = new LoggedTunableNumber( "Swerve/currentLimiting/ClosedLoop", Constants.DRIVE_RAMP_RATE_LIMIT); @@ -120,7 +120,7 @@ public void periodic() { SmartShuffleboard.put("Drive", "BL ABS Pos", backLeft.getAbsPosition()); SmartShuffleboard.put("Drive", "BR ABS Pos", backRight.getAbsPosition()); } - if(Constants.SWERVE_DEBUG){ + if(Constants.TUNING_MODE){ LoggedTunableNumber.ifChanged( hashCode(), () -> { diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java index 6ab179f0..827dae56 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java @@ -12,14 +12,12 @@ public class SparkMaxDriveMotorIO implements SwerveDriveMotorIO { private final SparkMax driveMotor; - private final SparkMaxConfig driveConfig; private final SparkMaxInputProvider inputProvider; public SparkMaxDriveMotorIO( int driveMotorIO, KinematicsConversionConfig conversionConfig, boolean driveInverted) { driveMotor = new SparkMax(driveMotorIO, SparkMax.MotorType.kBrushless); inputProvider = new SparkMaxInputProvider(driveMotor); - driveConfig = new SparkMaxConfig(); setMotorConfig(driveInverted); setConversionFactors(conversionConfig); } @@ -30,6 +28,7 @@ public void setDriveVoltage(double volts) { } private void setConversionFactors(KinematicsConversionConfig conversionConfig) { + SparkMaxConfig driveConfig = new SparkMaxConfig(); double driveVelConvFactor = (2 * conversionConfig.getWheelRadius() * Math.PI) / (conversionConfig.getProfile().getDriveGearRatio() * 60); @@ -47,6 +46,7 @@ private void setConversionFactors(KinematicsConversionConfig conversionConfig) { } private void setMotorConfig(boolean driveInverted) { + SparkMaxConfig driveConfig = new SparkMaxConfig(); driveConfig .inverted(driveInverted) .idleMode(IdleMode.kBrake) @@ -73,6 +73,7 @@ public void updateInputs(MotorInputs inputs) { @Override public void updateConfig( double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) { + SparkMaxConfig driveConfig = new SparkMaxConfig(); driveConfig .closedLoopRampRate(closedLoopRampRate) .secondaryCurrentLimit(secondaryCurrentLimit) From e52a8b626195b889fe6425ed5e3c47f505b5bf5d Mon Sep 17 00:00:00 2001 From: codetoad Date: Tue, 25 Feb 2025 18:33:00 -0500 Subject: [PATCH 14/20] built --- .../frc/robot/constants/GameConstants.java | 18 +- .../subsystems/swervev3/SwerveDrivetrain.java | 155 +++++++++--------- .../swervev3/io/drive/SwerveDriveMotorIO.java | 3 +- 3 files changed, 88 insertions(+), 88 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 1b911278..f699f326 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -21,15 +21,15 @@ public class GameConstants { public static final double GYRO_DIAGS_ANGLE = 30; // Debug - public static final boolean SWERVE_DEBUG = true; - public static final boolean INTAKE_DEBUG = true; - public static final boolean CLIMBER_DEBUG = true; - public static final boolean ELEVATOR_DEBUG = true; - public static final boolean CORAL_DEBUG = true; - public static final boolean HIHI_DEBUG = true; - public static final boolean BYEBYE_DEBUG = true; - public static final boolean COMMAND_DEBUG = true; - public static final boolean INPUTS_DEBUG = true; + public static final boolean SWERVE_DEBUG = false; + public static final boolean INTAKE_DEBUG = false; + public static final boolean CLIMBER_DEBUG = false; + public static final boolean ELEVATOR_DEBUG = false; + public static final boolean CORAL_DEBUG = false; + public static final boolean HIHI_DEBUG = false; + public static final boolean BYEBYE_DEBUG = false; + public static final boolean COMMAND_DEBUG = false; + public static final boolean INPUTS_DEBUG = false; public static final boolean TUNING_MODE = true; // Speeds diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 1753ccc7..29ec4bd8 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -70,26 +70,26 @@ public SwerveDrivetrain( this.poseEstimator = new PoseEstimator( frontLeft, frontRight, backLeft, backRight, apriltagIO, kinematics, getLastGyro()); - if(Constants.TUNING_MODE){ - closedLoopTunable = - new LoggedTunableNumber( - "Swerve/currentLimiting/ClosedLoop", Constants.DRIVE_RAMP_RATE_LIMIT); - smartLimitTunable = - new LoggedTunableNumber("Swerve/currentLimiting/SmartLimit", Constants.DRIVE_SMART_LIMIT); - secondaryLimitTunable = - new LoggedTunableNumber( - "Swerve/currentLimiting/SecondaryLimit", Constants.DRIVE_SECONDARY_LIMIT); - drivePTunable = new LoggedTunableNumber("Swerve/drive/P", Constants.DRIVE_PID_P); - driveITunable = new LoggedTunableNumber("Swerve/drive/I", Constants.DRIVE_PID_I); - driveDTunable = new LoggedTunableNumber("Swerve/drive/D", Constants.DRIVE_PID_D); - steerPTunable = new LoggedTunableNumber("Swerve/steer/P", Constants.STEER_PID_P); - steerITunable = new LoggedTunableNumber("Swerve/steer/I", Constants.STEER_PID_I); - steerDTunable = new LoggedTunableNumber("Swerve/steer/D", Constants.STEER_PID_D); - steerMaxAccelerationTunable = - new LoggedTunableNumber("Swerve/steer/maxAccel", Constants.MAX_ANGULAR_SPEED * 150); - steerMaxVelocityTunable = - new LoggedTunableNumber("Swerve/steer/maxVelocity", 2 * Math.PI * 150); - } + if (Constants.TUNING_MODE) { + closedLoopTunable = + new LoggedTunableNumber( + "Swerve/currentLimiting/ClosedLoop", Constants.DRIVE_RAMP_RATE_LIMIT); + smartLimitTunable = + new LoggedTunableNumber("Swerve/currentLimiting/SmartLimit", Constants.DRIVE_SMART_LIMIT); + secondaryLimitTunable = + new LoggedTunableNumber( + "Swerve/currentLimiting/SecondaryLimit", Constants.DRIVE_SECONDARY_LIMIT); + drivePTunable = new LoggedTunableNumber("Swerve/drive/P", Constants.DRIVE_PID_P); + driveITunable = new LoggedTunableNumber("Swerve/drive/I", Constants.DRIVE_PID_I); + driveDTunable = new LoggedTunableNumber("Swerve/drive/D", Constants.DRIVE_PID_D); + steerPTunable = new LoggedTunableNumber("Swerve/steer/P", Constants.STEER_PID_P); + steerITunable = new LoggedTunableNumber("Swerve/steer/I", Constants.STEER_PID_I); + steerDTunable = new LoggedTunableNumber("Swerve/steer/D", Constants.STEER_PID_D); + steerMaxAccelerationTunable = + new LoggedTunableNumber("Swerve/steer/maxAccel", Constants.MAX_ANGULAR_SPEED * 150); + steerMaxVelocityTunable = + new LoggedTunableNumber("Swerve/steer/maxVelocity", 2 * Math.PI * 150); + } } @Override @@ -120,61 +120,63 @@ public void periodic() { SmartShuffleboard.put("Drive", "BL ABS Pos", backLeft.getAbsPosition()); SmartShuffleboard.put("Drive", "BR ABS Pos", backRight.getAbsPosition()); } - if(Constants.TUNING_MODE){ - LoggedTunableNumber.ifChanged( - hashCode(), - () -> { - updateConfig( - closedLoopTunable.get(), secondaryLimitTunable.get(), (int) smartLimitTunable.get()); - }, - closedLoopTunable, - secondaryLimitTunable, - smartLimitTunable); - LoggedTunableNumber.ifChanged( - hashCode(), - () -> { - frontLeft.setDrivePID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); - frontRight.setDrivePID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); - backLeft.setDrivePID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); - backRight.setDrivePID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); - }, - drivePTunable, - driveITunable, - driveDTunable); - LoggedTunableNumber.ifChanged( - hashCode(), - () -> { - frontLeft.setSteerPID( - steerPTunable.get(), - steerITunable.get(), - steerDTunable.get(), - steerMaxAccelerationTunable.get(), - steerMaxVelocityTunable.get()); - frontRight.setSteerPID( - steerPTunable.get(), - steerITunable.get(), - steerDTunable.get(), - steerMaxAccelerationTunable.get(), - steerMaxVelocityTunable.get()); - backLeft.setSteerPID( - steerPTunable.get(), - steerITunable.get(), - steerDTunable.get(), - steerMaxAccelerationTunable.get(), - steerMaxVelocityTunable.get()); - backRight.setSteerPID( - steerPTunable.get(), - steerITunable.get(), - steerDTunable.get(), - steerMaxAccelerationTunable.get(), - steerMaxVelocityTunable.get()); - }, - steerPTunable, - steerITunable, - steerDTunable, - steerMaxAccelerationTunable, - steerMaxVelocityTunable); - } + if (Constants.TUNING_MODE) { + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + updateConfig( + closedLoopTunable.get(), + secondaryLimitTunable.get(), + (int) smartLimitTunable.get()); + }, + closedLoopTunable, + secondaryLimitTunable, + smartLimitTunable); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + frontLeft.setDrivePID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); + frontRight.setDrivePID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); + backLeft.setDrivePID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); + backRight.setDrivePID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); + }, + drivePTunable, + driveITunable, + driveDTunable); + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + frontLeft.setSteerPID( + steerPTunable.get(), + steerITunable.get(), + steerDTunable.get(), + steerMaxAccelerationTunable.get(), + steerMaxVelocityTunable.get()); + frontRight.setSteerPID( + steerPTunable.get(), + steerITunable.get(), + steerDTunable.get(), + steerMaxAccelerationTunable.get(), + steerMaxVelocityTunable.get()); + backLeft.setSteerPID( + steerPTunable.get(), + steerITunable.get(), + steerDTunable.get(), + steerMaxAccelerationTunable.get(), + steerMaxVelocityTunable.get()); + backRight.setSteerPID( + steerPTunable.get(), + steerITunable.get(), + steerDTunable.get(), + steerMaxAccelerationTunable.get(), + steerMaxVelocityTunable.get()); + }, + steerPTunable, + steerITunable, + steerDTunable, + steerMaxAccelerationTunable, + steerMaxVelocityTunable); + } } private void processInputs() { @@ -298,8 +300,7 @@ public boolean isFacingTarget() { * @param secondaryCurrentLimit * @param smartCurrentLimit */ - public void updateConfig( - double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) { + public void updateConfig(double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) { frontLeft.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); frontRight.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); backRight.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java index 69cf49a9..dae40c8c 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SwerveDriveMotorIO.java @@ -8,6 +8,5 @@ public interface SwerveDriveMotorIO extends LoggableIO { void resetEncoder(); - void updateConfig( - double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit); + void updateConfig(double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit); } From b96643e1be74e2d2697cfebc84c6a928e1ce8d9e Mon Sep 17 00:00:00 2001 From: codetoad Date: Tue, 25 Feb 2025 18:48:57 -0500 Subject: [PATCH 15/20] use noReset --- .../subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java index 827dae56..234b2a61 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/drive/SparkMaxDriveMotorIO.java @@ -41,7 +41,7 @@ private void setConversionFactors(KinematicsConversionConfig conversionConfig) { .velocityConversionFactor(driveVelConvFactor); driveMotor.configure( driveConfig, - SparkBase.ResetMode.kResetSafeParameters, + SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kPersistParameters); } @@ -56,7 +56,7 @@ private void setMotorConfig(boolean driveInverted) { Constants.DRIVE_SMART_LIMIT); // TODO: change current limiting because its different driveMotor.configure( driveConfig, - SparkBase.ResetMode.kResetSafeParameters, + SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kPersistParameters); } From c55c8dd858665d09c83d61354938cabf8ddf4a01 Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Feb 2025 10:10:31 -0500 Subject: [PATCH 16/20] added kV and kS --- .../frc/robot/constants/GameConstants.java | 4 +- .../subsystems/swervev3/SwerveDrivetrain.java | 41 ++++++++++++++++--- .../subsystems/swervev3/io/SwerveModule.java | 10 +++-- 3 files changed, 42 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index eeec5cf9..8d57106b 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -130,10 +130,8 @@ public enum Mode { public static final double DRIVE_PID_I = 0; // TODO: change later public static final double DRIVE_PID_D = 0; // TODO: change later public static final double DRIVE_PID_FF_S = 1; // TODO: change later - public static final double DRIVE_PID_FF_V = 2.8; // TODO: change later + public static final double DRIVE_PID_FF_V = 3.12; // TODO: change later public static final double DRIVE_PID_I_ZONE = 0; // TODO: change later - public static final double DRIVE_PID_MAX_VEL = 0; - public static final double DRIVE_PID_MAX_ACC = 0; public static final double DRIVE_PID_ALLOWED_ERROR = 0; // Steer PID diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 29ec4bd8..66c6f4d8 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -49,6 +49,8 @@ public class SwerveDrivetrain extends SubsystemBase { private LoggedTunableNumber drivePTunable; private LoggedTunableNumber driveITunable; private LoggedTunableNumber driveDTunable; + private LoggedTunableNumber driveKvTunable; + private LoggedTunableNumber driveKsTunable; private LoggedTunableNumber steerPTunable; private LoggedTunableNumber steerITunable; private LoggedTunableNumber steerDTunable; @@ -82,6 +84,10 @@ public SwerveDrivetrain( drivePTunable = new LoggedTunableNumber("Swerve/drive/P", Constants.DRIVE_PID_P); driveITunable = new LoggedTunableNumber("Swerve/drive/I", Constants.DRIVE_PID_I); driveDTunable = new LoggedTunableNumber("Swerve/drive/D", Constants.DRIVE_PID_D); + + driveKvTunable = new LoggedTunableNumber("Swerve/drive/Kv", Constants.DRIVE_PID_FF_V); + driveKsTunable = new LoggedTunableNumber("Swerve/drive/Ks", Constants.DRIVE_PID_FF_S); + steerPTunable = new LoggedTunableNumber("Swerve/steer/P", Constants.STEER_PID_P); steerITunable = new LoggedTunableNumber("Swerve/steer/I", Constants.STEER_PID_I); steerDTunable = new LoggedTunableNumber("Swerve/steer/D", Constants.STEER_PID_D); @@ -135,14 +141,36 @@ public void periodic() { LoggedTunableNumber.ifChanged( hashCode(), () -> { - frontLeft.setDrivePID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); - frontRight.setDrivePID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); - backLeft.setDrivePID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); - backRight.setDrivePID(drivePTunable.get(), driveITunable.get(), driveDTunable.get()); + frontLeft.setDrivePID( + drivePTunable.get(), + driveITunable.get(), + driveDTunable.get(), + driveKvTunable.get(), + driveKsTunable.get()); + frontRight.setDrivePID( + drivePTunable.get(), + driveITunable.get(), + driveDTunable.get(), + driveKvTunable.get(), + driveKsTunable.get()); + backLeft.setDrivePID( + drivePTunable.get(), + driveITunable.get(), + driveDTunable.get(), + driveKvTunable.get(), + driveKsTunable.get()); + backRight.setDrivePID( + drivePTunable.get(), + driveITunable.get(), + driveDTunable.get(), + driveKvTunable.get(), + driveKsTunable.get()); }, drivePTunable, driveITunable, - driveDTunable); + driveDTunable, + driveKvTunable, + driveKsTunable); LoggedTunableNumber.ifChanged( hashCode(), () -> { @@ -300,7 +328,8 @@ public boolean isFacingTarget() { * @param secondaryCurrentLimit * @param smartCurrentLimit */ - public void updateConfig(double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) { + public void updateConfig( + double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) { frontLeft.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); frontRight.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); backRight.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java index 2cb90c2b..85bb9f6d 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java @@ -156,13 +156,15 @@ private double getSteerPosition() { steerSystem.getInputs().getEncoderPosition() - steerOffset); } - public void setDrivePID(double P, double I, double D) { - drivePIDController.setPID(P, I, D); + public void setDrivePID(double p, double i, double d, double kV, double kS) { + drivePIDController.setPID(p, i, d); + driveFeedforward.setKv(kV); + driveFeedforward.setKs(kS); } public void setSteerPID( - double P, double I, double D, double maxAcceleration, double maxVelocity) { - turningPIDController.setPID(P, I, D); + double p, double i, double d, double maxAcceleration, double maxVelocity) { + turningPIDController.setPID(p, i, d); turningPIDController.setConstraints( new TrapezoidProfile.Constraints(maxAcceleration, maxVelocity)); } From 6f3989e948969241863b9750cf303d43e7d00dcb Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Feb 2025 10:19:55 -0500 Subject: [PATCH 17/20] added run with Volts --- src/main/java/frc/robot/RobotContainer.java | 10 ++++- .../commands/drivetrain/DriveVoltage.java | 41 +++++++++++++++++++ .../subsystems/swervev3/SwerveDrivetrain.java | 15 +++---- .../subsystems/swervev3/io/SwerveModule.java | 4 ++ 4 files changed, 62 insertions(+), 8 deletions(-) create mode 100644 src/main/java/frc/robot/commands/drivetrain/DriveVoltage.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 20194e84..784db988 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,6 +23,7 @@ import frc.robot.commands.coral.IntakeCoral; import frc.robot.commands.coral.ShootCoral; import frc.robot.commands.drivetrain.Drive; +import frc.robot.commands.drivetrain.DriveVoltage; import frc.robot.commands.drivetrain.RobotSlide; import frc.robot.commands.elevator.*; import frc.robot.commands.hihi.*; @@ -302,7 +303,14 @@ public SwerveDrivetrain getDrivetrain() { } public void putShuffleboardCommands() { - + if (Constants.SWERVE_DEBUG) { + SmartShuffleboard.put("Drive", "VoltsToRunWith", 0); + SmartShuffleboard.putCommand( + "Drive", + "RunWithVolts", + new DriveVoltage( + drivetrain, () -> SmartShuffleboard.getDouble("Drive", "VoltsToRunWith", 0), 1)); + } if (Constants.CORAL_DEBUG) { SmartShuffleboard.putCommand( "Commands", "Shoot Coral", new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED)); diff --git a/src/main/java/frc/robot/commands/drivetrain/DriveVoltage.java b/src/main/java/frc/robot/commands/drivetrain/DriveVoltage.java new file mode 100644 index 00000000..02cda8a1 --- /dev/null +++ b/src/main/java/frc/robot/commands/drivetrain/DriveVoltage.java @@ -0,0 +1,41 @@ +package frc.robot.commands.drivetrain; + +import edu.wpi.first.wpilibj.Timer; +import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.logging.commands.LoggableCommand; +import java.util.function.DoubleSupplier; + +/** + * This class can be used to calculate kS value for {@link + * edu.wpi.first.math.controller.SimpleMotorFeedforward SimpleMotorFeedforward}.
+ * Keep applying volts until the motors are able to move the robot at all.
+ * kS represents the volts need to overcome static friction + */ +public class DriveVoltage extends LoggableCommand { + private final SwerveDrivetrain drivetrain; + private final DoubleSupplier volts; + private final double time; + private final Timer timer; + + public DriveVoltage(SwerveDrivetrain drivetrain, DoubleSupplier volts, double time) { + this.drivetrain = drivetrain; + this.volts = volts; + this.time = time; + this.timer = new Timer(); + } + + @Override + public void initialize() { + timer.restart(); + } + + @Override + public void execute() { + drivetrain.applyVolts(volts.getAsDouble()); + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(time); + } +} diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index 66c6f4d8..c62c2202 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -120,12 +120,6 @@ public void periodic() { frontRight.getLatestState(), backLeft.getLatestState(), backRight.getLatestState()); - if (Constants.SWERVE_DEBUG) { - SmartShuffleboard.put("Drive", "FL ABS Pos", frontLeft.getAbsPosition()); - SmartShuffleboard.put("Drive", "FR ABS Pos", frontRight.getAbsPosition()); - SmartShuffleboard.put("Drive", "BL ABS Pos", backLeft.getAbsPosition()); - SmartShuffleboard.put("Drive", "BR ABS Pos", backRight.getAbsPosition()); - } if (Constants.TUNING_MODE) { LoggedTunableNumber.ifChanged( hashCode(), @@ -332,7 +326,14 @@ public void updateConfig( double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) { frontLeft.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); frontRight.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); - backRight.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); backLeft.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); + backRight.updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); + } + + public void applyVolts(double volts) { + frontLeft.applyVolts(volts); + frontRight.applyVolts(volts); + backLeft.applyVolts(volts); + backRight.applyVolts(volts); } } diff --git a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java index 85bb9f6d..0b29c47e 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swervev3/io/SwerveModule.java @@ -173,4 +173,8 @@ public void updateConfig( double closedLoopRampRate, double secondaryCurrentLimit, int smartCurrentLimit) { driveSystem.getIO().updateConfig(closedLoopRampRate, secondaryCurrentLimit, smartCurrentLimit); } + + public void applyVolts(double volts) { + driveSystem.getIO().setDriveVoltage(volts); + } } From 688afc9dadc91edfb8d1cd5b737202b4b4afdee5 Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Feb 2025 18:19:04 -0500 Subject: [PATCH 18/20] pid :) --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/constants/GameConstants.java | 8 ++++---- .../frc/robot/subsystems/swervev3/SwerveDrivetrain.java | 1 - 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 784db988..71f71761 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -309,7 +309,7 @@ public void putShuffleboardCommands() { "Drive", "RunWithVolts", new DriveVoltage( - drivetrain, () -> SmartShuffleboard.getDouble("Drive", "VoltsToRunWith", 0), 1)); + drivetrain, () -> SmartShuffleboard.getDouble("Drive", "VoltsToRunWith", 0), 5)); } if (Constants.CORAL_DEBUG) { SmartShuffleboard.putCommand( diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 8d57106b..44a8010d 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -30,7 +30,7 @@ public class GameConstants { public static final boolean BYEBYE_DEBUG = false; public static final boolean COMMAND_DEBUG = false; public static final boolean INPUTS_DEBUG = false; - public static final boolean TUNING_MODE = false; + public static final boolean TUNING_MODE = true; // Speeds public static final double MAX_AUTO_ALIGN_SPEED = 0.9; @@ -126,11 +126,11 @@ public enum Mode { public static final boolean ELEVATOR_USE_MAX_MOTION = true; // Drive PID - public static final double DRIVE_PID_P = 1; // TODO: change later + public static final double DRIVE_PID_P = 3; // TODO: change later public static final double DRIVE_PID_I = 0; // TODO: change later public static final double DRIVE_PID_D = 0; // TODO: change later - public static final double DRIVE_PID_FF_S = 1; // TODO: change later - public static final double DRIVE_PID_FF_V = 3.12; // TODO: change later + public static final double DRIVE_PID_FF_S = 0.19; + public static final double DRIVE_PID_FF_V = 3.3; public static final double DRIVE_PID_I_ZONE = 0; // TODO: change later public static final double DRIVE_PID_ALLOWED_ERROR = 0; diff --git a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java index c62c2202..d384d6c5 100644 --- a/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swervev3/SwerveDrivetrain.java @@ -19,7 +19,6 @@ import frc.robot.utils.logging.LoggableIO; import frc.robot.utils.logging.LoggedTunableNumber; import frc.robot.utils.logging.subsystem.LoggableSystem; -import frc.robot.utils.shuffleboard.SmartShuffleboard; import org.littletonrobotics.junction.Logger; public class SwerveDrivetrain extends SubsystemBase { From 6880c402e49e47175a11d86104085e84cf954359 Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Feb 2025 18:20:35 -0500 Subject: [PATCH 19/20] halfed robot centric drive --- src/main/java/frc/robot/commands/drivetrain/RobotSlide.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/drivetrain/RobotSlide.java b/src/main/java/frc/robot/commands/drivetrain/RobotSlide.java index 40f8c279..1173dc70 100644 --- a/src/main/java/frc/robot/commands/drivetrain/RobotSlide.java +++ b/src/main/java/frc/robot/commands/drivetrain/RobotSlide.java @@ -22,7 +22,7 @@ public RobotSlide(SwerveDrivetrain drivetrain, DoubleSupplier horizSupplier) { @Override public void execute() { double horizVal = -horizSupplier.getAsDouble(); - double str = MathUtil.applyDeadband(horizVal, 0.05) * Constants.MAX_VELOCITY; + double str = MathUtil.applyDeadband(horizVal, 0.05) * Constants.MAX_VELOCITY/2; ChassisSpeeds speeds = drivetrain.createChassisSpeeds(0, str, 0, DriveMode.ROBOT_CENTRIC); drivetrain.drive(speeds); } From 45fe91b978a9fd95d4728a64e7cfe30d311be8ab Mon Sep 17 00:00:00 2001 From: codetoad Date: Wed, 26 Feb 2025 18:25:40 -0500 Subject: [PATCH 20/20] pid fix --- src/main/java/frc/robot/commands/drivetrain/RobotSlide.java | 2 +- src/main/java/frc/robot/constants/GameConstants.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/drivetrain/RobotSlide.java b/src/main/java/frc/robot/commands/drivetrain/RobotSlide.java index 1173dc70..47ebe7c9 100644 --- a/src/main/java/frc/robot/commands/drivetrain/RobotSlide.java +++ b/src/main/java/frc/robot/commands/drivetrain/RobotSlide.java @@ -22,7 +22,7 @@ public RobotSlide(SwerveDrivetrain drivetrain, DoubleSupplier horizSupplier) { @Override public void execute() { double horizVal = -horizSupplier.getAsDouble(); - double str = MathUtil.applyDeadband(horizVal, 0.05) * Constants.MAX_VELOCITY/2; + double str = MathUtil.applyDeadband(horizVal, 0.05) * Constants.MAX_VELOCITY / 2; ChassisSpeeds speeds = drivetrain.createChassisSpeeds(0, str, 0, DriveMode.ROBOT_CENTRIC); drivetrain.drive(speeds); } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 44a8010d..caea8e2a 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -30,7 +30,7 @@ public class GameConstants { public static final boolean BYEBYE_DEBUG = false; public static final boolean COMMAND_DEBUG = false; public static final boolean INPUTS_DEBUG = false; - public static final boolean TUNING_MODE = true; + public static final boolean TUNING_MODE = false; // Speeds public static final double MAX_AUTO_ALIGN_SPEED = 0.9; @@ -126,7 +126,7 @@ public enum Mode { public static final boolean ELEVATOR_USE_MAX_MOTION = true; // Drive PID - public static final double DRIVE_PID_P = 3; // TODO: change later + public static final double DRIVE_PID_P = 2; // TODO: change later public static final double DRIVE_PID_I = 0; // TODO: change later public static final double DRIVE_PID_D = 0; // TODO: change later public static final double DRIVE_PID_FF_S = 0.19;