Skip to content

Gains tuning utility class #217

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

Open
iamawesomecat wants to merge 9 commits into
base: main
Choose a base branch
from
Open

Gains tuning utility class #217

Show file tree
Hide file tree
Changes from 7 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
e81a82a
Gains tuning utility class
iamawesomecat Dec 4, 2025
bbca421
whoops
iamawesomecat Dec 4, 2025
d4094ab
Fixing issues
iamawesomecat Dec 4, 2025
cdc5d0b
Merge branch 'GainsTuningEntry' of https://github.com/robototes/Reefs…
iamawesomecat Dec 4, 2025
be1cd96
spotless 👎😔😔😔😔
iamawesomecat Dec 4, 2025
3d34a60
added a gainsChanged method
iamawesomecat Dec 6, 2025
887710a
fine ill change the command
iamawesomecat Dec 6, 2025
208b36e
fixing gemini's issues
iamawesomecat Dec 6, 2025
2719a3c
spotless
iamawesomecat Dec 6, 2025
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
73 changes: 59 additions & 14 deletions src/main/java/frc/robot/subsystems/ArmPivot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
import static edu.wpi.first.units.Units.Seconds;
import static edu.wpi.first.units.Units.Volts;

import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfigurator;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
Expand All @@ -28,17 +29,18 @@
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import frc.robot.Hardware;
import frc.robot.util.GainsTuningEntry;
import java.util.function.Supplier;

public class ArmPivot extends SubsystemBase {
// Presets
private final double ARMPIVOT_KP = 38.5; // previously 50
private final double ARMPIVOT_KI = 0;
private final double ARMPIVOT_KD = 0;
private final double ARMPIVOT_KS = 0.1;
private final double ARMPIVOT_KV = 0.69;
private final double ARMPIVOT_KG = 0.18;
private final double ARMPIVOT_KA = 0.0;
private double armPivot_kP = 38.5; // previously 50
private double armPivot_kI = 0;
private double armPivot_kD = 0;
private double armPivot_kS = 0.1;
private double armPivot_kV = 0.69;
private double armPivot_kG = 0.18;
private double armPivot_kA = 0.0;
Comment thread
iamawesomecat marked this conversation as resolved.
// Preset positions for Arm with Coral
public static final double CORAL_PRESET_L1 = 0;
public static final double CORAL_PRESET_L2 = 0.13;
Expand Down Expand Up @@ -75,6 +77,18 @@ public class ArmPivot extends SubsystemBase {
// TalonFX
private final TalonFX motor;

// Testing tuning entry for PID gains
private final GainsTuningEntry PIDtuner =
new GainsTuningEntry(
"ArmPivot",
armPivot_kP,
armPivot_kI,
armPivot_kD,
armPivot_kA,
armPivot_kV,
armPivot_kS,
armPivot_kG);

// alerts if motor is not connected.
private final Alert NotConnectedError =
new Alert("ArmPivot", "Motor not connected", AlertType.kError);
Expand Down Expand Up @@ -202,13 +216,13 @@ public void factoryDefaults() {

// PID
// set slot 0 gains
talonFXConfiguration.Slot0.kS = ARMPIVOT_KS;
talonFXConfiguration.Slot0.kV = ARMPIVOT_KV;
talonFXConfiguration.Slot0.kA = ARMPIVOT_KA;
talonFXConfiguration.Slot0.kP = ARMPIVOT_KP;
talonFXConfiguration.Slot0.kI = ARMPIVOT_KI;
talonFXConfiguration.Slot0.kD = ARMPIVOT_KD;
talonFXConfiguration.Slot0.kG = ARMPIVOT_KG;
talonFXConfiguration.Slot0.kS = armPivot_kS;
talonFXConfiguration.Slot0.kV = armPivot_kV;
talonFXConfiguration.Slot0.kA = armPivot_kA;
talonFXConfiguration.Slot0.kP = armPivot_kP;
talonFXConfiguration.Slot0.kI = armPivot_kI;
talonFXConfiguration.Slot0.kD = armPivot_kD;
talonFXConfiguration.Slot0.kG = armPivot_kG;
talonFXConfiguration.Slot0.GravityType = GravityTypeValue.Arm_Cosine;

// set Motion Magic settings in rps not mechanism units
Expand All @@ -227,6 +241,37 @@ public void factoryDefaults() {
public void periodic() {
// Error that ensures the motor is connected
NotConnectedError.set(notConnectedDebouncer.calculate(!motor.getMotorVoltage().hasUpdated()));

// checking if any of the PID gains have changed in gains tuner
if (PIDtuner.anyGainsChanged(
armPivot_kP,
armPivot_kI,
armPivot_kD,
armPivot_kA,
armPivot_kV,
armPivot_kS,
armPivot_kG)) {
armPivot_kP = PIDtuner.getP();
armPivot_kI = PIDtuner.getI();
armPivot_kD = PIDtuner.getD();
armPivot_kS = PIDtuner.getS();
armPivot_kV = PIDtuner.getV();
armPivot_kA = PIDtuner.getA();
armPivot_kG = PIDtuner.getG();
Copy link
Copy Markdown
Contributor

@Jetblackdragon Jetblackdragon Dec 9, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Instead of doing this periodically you could add a button that updates your values. For example you add button in elastic that says update and it pushes the new values instead of constantly checking and updating the value in 2 places. Its also safety incase you reconfigure it while its running a command (Im not sure what the priority on that is tho). Also unnecessary but something to consider.


// creating a new Slot0Configs object to apply the new gains
Slot0Configs pidTunerConfigs = new Slot0Configs();
pidTunerConfigs.kP = armPivot_kP;
pidTunerConfigs.kI = armPivot_kI;
pidTunerConfigs.kD = armPivot_kD;
pidTunerConfigs.kS = armPivot_kS;
pidTunerConfigs.kV = armPivot_kV;
pidTunerConfigs.kA = armPivot_kA;
pidTunerConfigs.kG = armPivot_kG;

// applying the new gains to the motor
motor.getConfigurator().apply(pidTunerConfigs);
Comment thread
iamawesomecat marked this conversation as resolved.
Copy link
Copy Markdown
Contributor

@markpete markpete Dec 9, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It might be better to just store the configurator object locally, so that you don't have to retrieve it every time through this loop. Because this loop is periodic, it can happen often, so you want to optimize for fast run-time.

if (!!m_Configurator) {
   m_Configurator = motor.getConfigurator();
}
m_Configurator.apply(pidTunerConfigs);

}
}
}
// -Samuel "Big North" Mallick
Expand Down
176 changes: 176 additions & 0 deletions src/main/java/frc/robot/util/GainsTuningEntry.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,176 @@
package frc.robot.util;

import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;

public class GainsTuningEntry {

private final NetworkTable table;
private String kSubsystemName;
private NetworkTableEntry kPEntry;
private NetworkTableEntry kIEntry;
private NetworkTableEntry kDEntry;
private NetworkTableEntry kAEntry;
private NetworkTableEntry kVEntry;
private NetworkTableEntry kSEntry;
private NetworkTableEntry kGEntry;
Comment thread
iamawesomecat marked this conversation as resolved.
Comment thread
iamawesomecat marked this conversation as resolved.

/* Base PID Feedback entires. Excludes kA, kV, kS, and kG
*
*/
Comment thread
iamawesomecat marked this conversation as resolved.
Outdated
public GainsTuningEntry(String subsystemName, double kp, double ki, double kd) {
table = NetworkTableInstance.getDefault().getTable("Gains/" + subsystemName);
this.kSubsystemName = subsystemName;
Copy link
Copy Markdown
Contributor

@markpete markpete Dec 9, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is there any validation you need to do on any of these parameters? e.g. null or empty subsystemName, negative or null values? What assumptions are you making about the data that need to be confirmed?

kPEntry = table.getEntry("kP");
kIEntry = table.getEntry("kI");
kDEntry = table.getEntry("kD");
kAEntry = null;
kVEntry = null;
kSEntry = null;
kGEntry = null;
kPEntry.setDouble(kp);
kIEntry.setDouble(ki);
kDEntry.setDouble(kd);
}

/* With ALL Feedback + Feedforward entries, INCLUDING kA
*
*/
public GainsTuningEntry(
String subsystemName,
double kp,
double ki,
double kd,
double ka,
double kv,
double ks,
double kg) {
this(subsystemName, kp, ki, kd);
kAEntry = table.getEntry("kA");
kVEntry = table.getEntry("kV");
kSEntry = table.getEntry("kS");
kGEntry = table.getEntry("kG");
kAEntry.setDouble(ka);
kVEntry.setDouble(kv);
kSEntry.setDouble(ks);
kGEntry.setDouble(kg);
}

/* With Feedback + Feedforward entries, except kA
*
*/
public GainsTuningEntry(
String subsystemName, double kp, double ki, double kd, double kv, double ks, double kg) {
this(subsystemName, kp, ki, kd);
kVEntry = table.getEntry("kV");
kSEntry = table.getEntry("kS");
kGEntry = table.getEntry("kG");
kVEntry.setDouble(kv);
kSEntry.setDouble(ks);
kGEntry.setDouble(kg);
}

public String getSubsystem() {
return kSubsystemName;
}

// Get proportional gain (P)
public double getP() {
return kPEntry.getDouble(0);
}

// Get integral gain (I)
public double getI() {
return kIEntry.getDouble(0);
}

// Get derivative gain (D)
public double getD() {
return kDEntry.getDouble(0);
}

// Get acceleration gain (A)
public double getA() {
if (kAEntry == null) {
return 0;
} else {
return kAEntry.getDouble(0);
}
Comment thread
iamawesomecat marked this conversation as resolved.
Outdated
}
Comment thread
iamawesomecat marked this conversation as resolved.

// Get velocity gain (V)
public double getV() {
if (kVEntry == null) {
return 0;
} else {
return kVEntry.getDouble(0);
}
}

// Get static gain (S)
public double getS() {
if (kSEntry == null) {
return 0;
} else {
return kSEntry.getDouble(0);
}
}

// Get gravity gain (G)
public double getG() {
if (kGEntry == null) {
return 0;
} else {
return kGEntry.getDouble(0);
}
}

/* Check if any gains have changed using the current values from the subsystem.
* Compares all gains: P, I, D, A, V, S, G
*/
public boolean anyGainsChanged(
double currentP,
double currentI,
double currentD,
double currentA,
double currentV,
double currentS,
double currentG) {

return (currentP != getP()
|| currentI != getI()
|| currentD != getD()
|| currentA != getA()
|| currentV != getV()
|| currentS != getS()
|| currentG != getG());
Comment thread
iamawesomecat marked this conversation as resolved.
Outdated
}

/* Check if any gains have changed using the current values from the subsystem.
* Compares P, I, D, V, S, G
*/
public boolean pidffGainsChanged(
double currentP,
double currentI,
double currentD,
double currentV,
double currentS,
double currentG) {

return (currentP != getP()
|| currentI != getI()
|| currentD != getD()
|| currentV != getV()
|| currentS != getS()
|| currentG != getG());
}

/* Check if any gains have changed using the current values from the subsystem.
* Compares P, I, D only
*/
public boolean pidGainsChanged(double currentP, double currentI, double currentD) {

return (currentP != getP() || currentI != getI() || currentD != getD());
}
Comment thread
iamawesomecat marked this conversation as resolved.
}