Skip to content

Swerve Tune #120

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
timtogan wants to merge 1 commit into
base: master
Choose a base branch
from
Draft

Swerve Tune #120

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 34 additions & 0 deletions src/main/java/org/carlmontrobotics/lib199/swerve/SwerveConfig.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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;
}

}
39 changes: 39 additions & 0 deletions src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<Float> pitchDegSupplier, Supplier<Float> rollDegSupplier) {
driveMotorType = MotorControllerType.getMotorControllerType(drive);
Expand Down Expand Up @@ -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() {
Expand Down Expand Up @@ -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));
}
}

/**
Expand Down