From 5dacca7b5c9c9ddaa1d9700cbd628664d6385ef1 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Mon, 19 Jan 2026 00:50:57 -0800 Subject: [PATCH] made it possible to tune the drivetrain through SmartDashboard --- .../lib199/swerve/SwerveConfig.java | 34 ++++++++++++++++ .../lib199/swerve/SwerveModule.java | 39 +++++++++++++++++++ 2 files changed, 73 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java index 1e79c15b..e92fc7b4 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java @@ -6,6 +6,7 @@ public final class SwerveConfig { public double[] kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZeroDeg; public boolean[] driveInversion, turnInversion, reversed; + public boolean tunning = false; public SwerveConfig(double wheelDiameterMeters, double driveGearing, double mu, double autoCentripetalAccel, double[] kForwardVolts, double[] kForwardVels, double[] kForwardAccels, @@ -38,4 +39,37 @@ public SwerveConfig(double wheelDiameterMeters, double driveGearing, double mu, this.turnInversion = turnInversion; } + + public SwerveConfig(double wheelDiameterMeters, double driveGearing, double mu, + double autoCentripetalAccel, double[] kForwardVolts, double[] kForwardVels, double[] kForwardAccels, + double[] kBackwardVolts, double[] kBackwardVels, double[] kBackwardAccels, double[] drivekP, + double[] drivekI, double[] drivekD, double[] turnkP, double[] turnkI, double[] turnkD, double[] turnkS, + double[] turnkV, double[] turnkA, double[] turnZeroDeg, boolean[] driveInversion, boolean[] reversed, double driveModifier, boolean[] turnInversion, boolean tunning) { + this.wheelDiameterMeters = wheelDiameterMeters; + this.driveGearing = driveGearing; + this.mu = mu;//coefficient of friction between the wheel and the surface + this.autoCentripetalAccel = autoCentripetalAccel; + this.kForwardVolts = kForwardVolts; + this.kForwardVels = kForwardVels; + this.kForwardAccels = kForwardAccels; + this.kBackwardVolts = kBackwardVolts; + this.kBackwardVels = kBackwardVels; + this.kBackwardAccels = kBackwardAccels; + this.drivekP = drivekP; + this.drivekI = drivekI; + this.drivekD = drivekD; + this.turnkP = turnkP; + this.turnkI = turnkI; + this.turnkD = turnkD; + this.turnkS = turnkS;//for overcoming static friction + this.turnkV = turnkV; + this.turnkA = turnkA; + this.turnZeroDeg = turnZeroDeg; + this.driveInversion = driveInversion; + this.reversed = reversed; + this.driveModifier = driveModifier; + this.turnInversion = turnInversion; + this.tunning = tunning; + } + } diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index 46c79b12..6f24efca 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -66,6 +66,8 @@ public enum ModuleType {FL, FR, BL, BR}; SparkBaseConfigAccessor driveConfigAccessor; SparkBaseConfigAccessor turnConfigAccessor; + private int arrIndex; + public SwerveModule(SwerveConfig config, ModuleType type, SparkBase drive, SparkBase turn, CANcoder turnEncoder, int arrIndex, Supplier pitchDegSupplier, Supplier rollDegSupplier) { driveMotorType = MotorControllerType.getMotorControllerType(drive); @@ -197,6 +199,25 @@ public void periodic() { drivePeriodic(); // updateSmartDashboard(); turnPeriodic(); + if(config.tunning){ + //drive + drivePIDController.setP(SmartDashboard.getNumber("kP", config.drivekP[arrIndex])); + drivePIDController.setI(SmartDashboard.getNumber("kI", config.drivekI[arrIndex])); + drivePIDController.setD(SmartDashboard.getNumber("kD", config.drivekD[arrIndex])); + //turn + turnPIDController.setP(SmartDashboard.getNumber("kP", config.turnkP[arrIndex])); + turnPIDController.setI(SmartDashboard.getNumber("kI", config.turnkI[arrIndex])); + turnPIDController.setD(SmartDashboard.getNumber("kD", config.turnkD[arrIndex])); + turnSimpleMotorFeedforward.setKs(SmartDashboard.getNumber("kS", config.turnkS[arrIndex])); + turnSimpleMotorFeedforward.setKv(SmartDashboard.getNumber("kV", config.turnkV[arrIndex])); + turnSimpleMotorFeedforward.setKa(SmartDashboard.getNumber("kA", config.turnkA[arrIndex])); + forwardSimpleMotorFF.setKs(SmartDashboard.getNumber("kForwardVolts", config.kForwardVolts[arrIndex])); + forwardSimpleMotorFF.setKv(SmartDashboard.getNumber("kForwardVels", config.kForwardVels[arrIndex])); + forwardSimpleMotorFF.setKa(SmartDashboard.getNumber("kForwardAccels", config.kForwardAccels[arrIndex])); + backwardSimpleMotorFF.setKs(SmartDashboard.getNumber("kBackwardVolts", config.kBackwardVolts[arrIndex])); + backwardSimpleMotorFF.setKv(SmartDashboard.getNumber("kBackwardVels", config.kBackwardVels[arrIndex])); + backwardSimpleMotorFF.setKa(SmartDashboard.getNumber("kBackwardAccels", config.kBackwardAccels[arrIndex])); + } } public void drivePeriodic() { @@ -485,6 +506,24 @@ public void initSendable(SendableBuilder builder) { builder.addDoubleProperty("Turn PID Output", () -> turnSpeedCorrectionVolts, null); builder.addDoubleProperty("Turn FF Output", () -> turnFFVolts, null); builder.addDoubleProperty("Turn Total Output", () -> turnVolts, null); + + if(config.tunning) { + builder.addDoubleProperty("drivekP", () -> drivePIDController.getP(), x -> drivePIDController.setP(x)); + builder.addDoubleProperty("drivekI", () -> drivePIDController.getI(), x -> drivePIDController.setI(x)); + builder.addDoubleProperty("drivekD", () -> drivePIDController.getD(), x -> drivePIDController.setD(x)); + builder.addDoubleProperty("turnkP", () -> turnPIDController.getP(), x -> turnPIDController.setP(x)); + builder.addDoubleProperty("turnkI", () -> turnPIDController.getI(), x -> turnPIDController.setI(x)); + builder.addDoubleProperty("turnkD", () -> turnPIDController.getD(), x -> turnPIDController.setD(x)); + builder.addDoubleProperty("turnkS", () -> turnSimpleMotorFeedforward.getKs(), x -> turnSimpleMotorFeedforward.setKs(x)); + builder.addDoubleProperty("turnkV", () -> turnSimpleMotorFeedforward.getKv(), x -> turnSimpleMotorFeedforward.setKv(x)); + builder.addDoubleProperty("turnkA", () -> turnSimpleMotorFeedforward.getKa(), x -> turnSimpleMotorFeedforward.setKa(x)); + builder.addDoubleProperty("kForwardVolts", () -> forwardSimpleMotorFF.getKs(), x -> forwardSimpleMotorFF.setKs(x)); + builder.addDoubleProperty("kForwardVels", () -> forwardSimpleMotorFF.getKv(), x -> forwardSimpleMotorFF.setKv(x)); + builder.addDoubleProperty("kForwardAccels", () -> forwardSimpleMotorFF.getKa(), x -> forwardSimpleMotorFF.setKa(x)); + builder.addDoubleProperty("kBackwardVolts", () -> backwardSimpleMotorFF.getKs(), x -> backwardSimpleMotorFF.setKs(x)); + builder.addDoubleProperty("kBackwardVels", () -> backwardSimpleMotorFF.getKv(), x -> backwardSimpleMotorFF.setKv(x)); + builder.addDoubleProperty("kBackwardAccels", () -> backwardSimpleMotorFF.getKa(), x -> backwardSimpleMotorFF.setKa(x)); + } } /**