Skip to content

Commit 5dacca7

Browse files
timtogantimtogan
committed
made it possible to tune the drivetrain through SmartDashboard
1 parent f30d84c commit 5dacca7

File tree

2 files changed

+73
-0
lines changed

2 files changed

+73
-0
lines changed

‎src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ public final class SwerveConfig {
66
public double[] kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, kBackwardAccels,
77
drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZeroDeg;
88
public boolean[] driveInversion, turnInversion, reversed;
9+
public boolean tunning = false;
910

1011
public SwerveConfig(double wheelDiameterMeters, double driveGearing, double mu,
1112
double autoCentripetalAccel, double[] kForwardVolts, double[] kForwardVels, double[] kForwardAccels,
@@ -38,4 +39,37 @@ public SwerveConfig(double wheelDiameterMeters, double driveGearing, double mu,
3839
this.turnInversion = turnInversion;
3940
}
4041

42+
43+
public SwerveConfig(double wheelDiameterMeters, double driveGearing, double mu,
44+
double autoCentripetalAccel, double[] kForwardVolts, double[] kForwardVels, double[] kForwardAccels,
45+
double[] kBackwardVolts, double[] kBackwardVels, double[] kBackwardAccels, double[] drivekP,
46+
double[] drivekI, double[] drivekD, double[] turnkP, double[] turnkI, double[] turnkD, double[] turnkS,
47+
double[] turnkV, double[] turnkA, double[] turnZeroDeg, boolean[] driveInversion, boolean[] reversed, double driveModifier, boolean[] turnInversion, boolean tunning) {
48+
this.wheelDiameterMeters = wheelDiameterMeters;
49+
this.driveGearing = driveGearing;
50+
this.mu = mu;//coefficient of friction between the wheel and the surface
51+
this.autoCentripetalAccel = autoCentripetalAccel;
52+
this.kForwardVolts = kForwardVolts;
53+
this.kForwardVels = kForwardVels;
54+
this.kForwardAccels = kForwardAccels;
55+
this.kBackwardVolts = kBackwardVolts;
56+
this.kBackwardVels = kBackwardVels;
57+
this.kBackwardAccels = kBackwardAccels;
58+
this.drivekP = drivekP;
59+
this.drivekI = drivekI;
60+
this.drivekD = drivekD;
61+
this.turnkP = turnkP;
62+
this.turnkI = turnkI;
63+
this.turnkD = turnkD;
64+
this.turnkS = turnkS;//for overcoming static friction
65+
this.turnkV = turnkV;
66+
this.turnkA = turnkA;
67+
this.turnZeroDeg = turnZeroDeg;
68+
this.driveInversion = driveInversion;
69+
this.reversed = reversed;
70+
this.driveModifier = driveModifier;
71+
this.turnInversion = turnInversion;
72+
this.tunning = tunning;
73+
}
74+
4175
}

‎src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java‎

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -66,6 +66,8 @@ public enum ModuleType {FL, FR, BL, BR};
6666
SparkBaseConfigAccessor driveConfigAccessor;
6767
SparkBaseConfigAccessor turnConfigAccessor;
6868

69+
private int arrIndex;
70+
6971
public SwerveModule(SwerveConfig config, ModuleType type, SparkBase drive, SparkBase turn, CANcoder turnEncoder,
7072
int arrIndex, Supplier<Float> pitchDegSupplier, Supplier<Float> rollDegSupplier) {
7173
driveMotorType = MotorControllerType.getMotorControllerType(drive);
@@ -197,6 +199,25 @@ public void periodic() {
197199
drivePeriodic();
198200
// updateSmartDashboard();
199201
turnPeriodic();
202+
if(config.tunning){
203+
//drive
204+
drivePIDController.setP(SmartDashboard.getNumber("kP", config.drivekP[arrIndex]));
205+
drivePIDController.setI(SmartDashboard.getNumber("kI", config.drivekI[arrIndex]));
206+
drivePIDController.setD(SmartDashboard.getNumber("kD", config.drivekD[arrIndex]));
207+
//turn
208+
turnPIDController.setP(SmartDashboard.getNumber("kP", config.turnkP[arrIndex]));
209+
turnPIDController.setI(SmartDashboard.getNumber("kI", config.turnkI[arrIndex]));
210+
turnPIDController.setD(SmartDashboard.getNumber("kD", config.turnkD[arrIndex]));
211+
turnSimpleMotorFeedforward.setKs(SmartDashboard.getNumber("kS", config.turnkS[arrIndex]));
212+
turnSimpleMotorFeedforward.setKv(SmartDashboard.getNumber("kV", config.turnkV[arrIndex]));
213+
turnSimpleMotorFeedforward.setKa(SmartDashboard.getNumber("kA", config.turnkA[arrIndex]));
214+
forwardSimpleMotorFF.setKs(SmartDashboard.getNumber("kForwardVolts", config.kForwardVolts[arrIndex]));
215+
forwardSimpleMotorFF.setKv(SmartDashboard.getNumber("kForwardVels", config.kForwardVels[arrIndex]));
216+
forwardSimpleMotorFF.setKa(SmartDashboard.getNumber("kForwardAccels", config.kForwardAccels[arrIndex]));
217+
backwardSimpleMotorFF.setKs(SmartDashboard.getNumber("kBackwardVolts", config.kBackwardVolts[arrIndex]));
218+
backwardSimpleMotorFF.setKv(SmartDashboard.getNumber("kBackwardVels", config.kBackwardVels[arrIndex]));
219+
backwardSimpleMotorFF.setKa(SmartDashboard.getNumber("kBackwardAccels", config.kBackwardAccels[arrIndex]));
220+
}
200221
}
201222

202223
public void drivePeriodic() {
@@ -485,6 +506,24 @@ public void initSendable(SendableBuilder builder) {
485506
builder.addDoubleProperty("Turn PID Output", () -> turnSpeedCorrectionVolts, null);
486507
builder.addDoubleProperty("Turn FF Output", () -> turnFFVolts, null);
487508
builder.addDoubleProperty("Turn Total Output", () -> turnVolts, null);
509+
510+
if(config.tunning) {
511+
builder.addDoubleProperty("drivekP", () -> drivePIDController.getP(), x -> drivePIDController.setP(x));
512+
builder.addDoubleProperty("drivekI", () -> drivePIDController.getI(), x -> drivePIDController.setI(x));
513+
builder.addDoubleProperty("drivekD", () -> drivePIDController.getD(), x -> drivePIDController.setD(x));
514+
builder.addDoubleProperty("turnkP", () -> turnPIDController.getP(), x -> turnPIDController.setP(x));
515+
builder.addDoubleProperty("turnkI", () -> turnPIDController.getI(), x -> turnPIDController.setI(x));
516+
builder.addDoubleProperty("turnkD", () -> turnPIDController.getD(), x -> turnPIDController.setD(x));
517+
builder.addDoubleProperty("turnkS", () -> turnSimpleMotorFeedforward.getKs(), x -> turnSimpleMotorFeedforward.setKs(x));
518+
builder.addDoubleProperty("turnkV", () -> turnSimpleMotorFeedforward.getKv(), x -> turnSimpleMotorFeedforward.setKv(x));
519+
builder.addDoubleProperty("turnkA", () -> turnSimpleMotorFeedforward.getKa(), x -> turnSimpleMotorFeedforward.setKa(x));
520+
builder.addDoubleProperty("kForwardVolts", () -> forwardSimpleMotorFF.getKs(), x -> forwardSimpleMotorFF.setKs(x));
521+
builder.addDoubleProperty("kForwardVels", () -> forwardSimpleMotorFF.getKv(), x -> forwardSimpleMotorFF.setKv(x));
522+
builder.addDoubleProperty("kForwardAccels", () -> forwardSimpleMotorFF.getKa(), x -> forwardSimpleMotorFF.setKa(x));
523+
builder.addDoubleProperty("kBackwardVolts", () -> backwardSimpleMotorFF.getKs(), x -> backwardSimpleMotorFF.setKs(x));
524+
builder.addDoubleProperty("kBackwardVels", () -> backwardSimpleMotorFF.getKv(), x -> backwardSimpleMotorFF.setKv(x));
525+
builder.addDoubleProperty("kBackwardAccels", () -> backwardSimpleMotorFF.getKa(), x -> backwardSimpleMotorFF.setKa(x));
526+
}
488527
}
489528

490529
/**

0 commit comments

Comments
 (0)