Skip to content

Commit

Permalink
Switch to TunableNumbers for gains
Browse files Browse the repository at this point in the history
Signed-off-by: Jade Turner <spacey-sooty@proton.me>
  • Loading branch information
spacey-sooty committed Feb 17, 2025
1 parent d69ba14 commit 7dc28f2
Show file tree
Hide file tree
Showing 7 changed files with 213 additions and 18 deletions.
1 change: 1 addition & 0 deletions src/main/java/org/curtinfrc/frc2025/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
*/
public final class Constants {
public static final RobotType robotType = RobotType.DEVBOT;
public static final boolean tuningMode = true;
public static final double ROBOT_X = 660; // mm
public static final double ROBOT_Y = 680;

Expand Down
Original file line number Diff line number Diff line change
@@ -1,22 +1,57 @@
package org.curtinfrc.frc2025.subsystems.ejector;

import static org.curtinfrc.frc2025.subsystems.ejector.EjectorConstants.*;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import org.curtinfrc.frc2025.Constants;
import org.curtinfrc.frc2025.util.LoggedTunableNumber;
import org.littletonrobotics.junction.Logger;

public class Ejector extends SubsystemBase {
private final EjectorIO io;
private final EjectorIOInputsAutoLogged inputs = new EjectorIOInputsAutoLogged();
private final PIDController pid = new PIDController(kP, 0, kD);
private final SimpleMotorFeedforward ff = new SimpleMotorFeedforward(kS, kV, kA);
private final PIDController pid = new PIDController(0, 0, 0);
private final SimpleMotorFeedforward ff = new SimpleMotorFeedforward(0, 0);

// Tunable Numbers
private final LoggedTunableNumber kS = new LoggedTunableNumber("Ejector/kS");
private final LoggedTunableNumber kV = new LoggedTunableNumber("Ejector/kV");
private final LoggedTunableNumber kP = new LoggedTunableNumber("Ejector/kP");
private final LoggedTunableNumber kD = new LoggedTunableNumber("Ejector/kD");

public Ejector(EjectorIO io) {
this.io = io;
switch (Constants.robotType) {
case COMPBOT:
kS.initDefault(0.8);
kV.initDefault(0.11);
ff.setKs(kS.get());
ff.setKv(kV.get());
kP.initDefault(0);
kD.initDefault(0);
pid.setPID(kP.get(), 0, kD.get());
break;
case DEVBOT:
kS.initDefault(0.8);
kV.initDefault(0.11);
ff.setKs(kS.get());
ff.setKv(kV.get());
kP.initDefault(0);
kD.initDefault(0);
pid.setPID(kP.get(), 0, kD.get());
break;
case SIMBOT:
kS.initDefault(0.8);
kV.initDefault(0.11);
ff.setKs(kS.get());
ff.setKv(kV.get());
kP.initDefault(0);
kD.initDefault(0);
pid.setPID(kP.get(), 0, kD.get());
break;
}
}

public final Trigger sensor = new Trigger(() -> inputs.sensor);
Expand All @@ -26,6 +61,18 @@ public Ejector(EjectorIO io) {
public void periodic() {
io.updateInputs(inputs);
Logger.processInputs("Ejector", inputs);

if (kS.hasChanged(hashCode())) {
ff.setKs(kS.get());
}

if (kV.hasChanged(hashCode())) {
ff.setKv(kV.get());
}

if (kP.hasChanged(hashCode()) || kD.hasChanged(hashCode())) {
pid.setPID(kP.get(), 0, kD.get());
}
}

public Command stop() {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,4 @@ public class EjectorConstants {
public static final int currentLimit = 60;
public static final double ejectorMoi = 0.025;
public static final double ejectorReduction = 1;
public static final double kP = 0;
public static final double kD = 0;
public static final double kS = 0.8;
public static final double kV = 0.11;
public static final double kA = 0;
}
Original file line number Diff line number Diff line change
Expand Up @@ -8,22 +8,52 @@
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import org.curtinfrc.frc2025.Constants;
import org.curtinfrc.frc2025.Constants.Setpoints;
import org.curtinfrc.frc2025.util.LoggedTunableNumber;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;

public class Elevator extends SubsystemBase {
private final ElevatorIO io;
private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged();
private final PIDController pid = new PIDController(kP, 0, kD);
private final PIDController pid = new PIDController(0, 0, 0);

@AutoLogOutput(key = "Elevator/Setpoint")
private Setpoints setpoint = Setpoints.COLLECT;

// Tunable Numbers
LoggedTunableNumber kP = new LoggedTunableNumber("Elevator/kP");
LoggedTunableNumber kD = new LoggedTunableNumber("Elevator/kD");

@AutoLogOutput(key = "Elevator/IsNotAtCollect")
public final Trigger isNotAtCollect = new Trigger(() -> setpoint != Setpoints.COLLECT);

public final Trigger toZero = new Trigger(() -> inputs.touchSensor);

@AutoLogOutput(key = "Elevator/AtSetpoint")
public Trigger atSetpoint = new Trigger(pid::atSetpoint);

public Elevator(ElevatorIO io) {
this.io = io;
pid.setTolerance(tolerance);
switch (Constants.robotType) {
case COMPBOT:
kP.initDefault(16);
kD.initDefault(0);
pid.setPID(kP.get(), 0, kD.get());
break;
case DEVBOT:
kP.initDefault(16);
kD.initDefault(0);
pid.setPID(kP.get(), 0, kD.get());
break;
case SIMBOT:
kP.initDefault(16);
kD.initDefault(0);
pid.setPID(kP.get(), 0, kD.get());
break;
}
}

@Override
Expand All @@ -34,9 +64,11 @@ public void periodic() {
Logger.recordOutput("Elevator/setpoint", setpoint);
Logger.recordOutput("Elevator/AtSetpoint", atSetpoint.getAsBoolean());
Logger.recordOutput("Elevator/ActualError", pid.getError());
}

public Trigger atSetpoint = new Trigger(pid::atSetpoint);
if (kP.hasChanged(hashCode()) || kD.hasChanged(hashCode())) {
pid.setPID(kP.get(), 0, kD.get());
}
}

public Command goToSetpoint(Setpoints point) {
return run(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -5,12 +5,11 @@ public class ElevatorConstants {
public static int motorPort = 31;

// TODO: TUNE PID and ff
public static double tolerance = 0.05;
public static double kP = 16;
public static double kD = 0;
// TODO: not using tunables for this yet
public static double kV = 0;
public static double kA = 0.01;
public static double kG = 0;
public static double tolerance = 1;

public static final double pulleyRadiusMeters = 0.002927789;
public static final double maxHeightMeters = 0.8;
Expand Down
Original file line number Diff line number Diff line change
@@ -1,7 +1,5 @@
package org.curtinfrc.frc2025.subsystems.elevator;

import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.kA;
import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.kV;
import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.maxHeightMeters;
import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.positionMetresToRotations;

Expand All @@ -10,7 +8,7 @@

public class ElevatorIOSim implements ElevatorIO {
private final ElevatorSim elevatorSim =
new ElevatorSim(kV, kA, DCMotor.getNEO(1), 0, maxHeightMeters, true, 0);
new ElevatorSim(0, 0.01, DCMotor.getNEO(1), 0, maxHeightMeters, true, 0);
private double volts = 0;

@Override
Expand Down
123 changes: 123 additions & 0 deletions src/main/java/org/curtinfrc/frc2025/util/LoggedTunableNumber.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,123 @@
// Copyright (c) 2025 FRC 6328
// http://github.com/Mechanical-Advantage
//
// Use of this source code is governed by an MIT-style
// license that can be found in the LICENSE file at
// the root directory of this project.

package org.curtinfrc.frc2025.util;

import java.util.Arrays;
import java.util.HashMap;
import java.util.Map;
import java.util.function.Consumer;
import java.util.function.DoubleSupplier;
import org.curtinfrc.frc2025.Constants;
import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;

/**
* Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or
* value not in dashboard.
*/
public class LoggedTunableNumber implements DoubleSupplier {
private static final String tableKey = "/Tuning";

private final String key;
private boolean hasDefault = false;
private double defaultValue;
private LoggedNetworkNumber dashboardNumber;
private Map<Integer, Double> lastHasChangedValues = new HashMap<>();

/**
* Create a new LoggedTunableNumber
*
* @param dashboardKey Key on dashboard
*/
public LoggedTunableNumber(String dashboardKey) {
this.key = tableKey + "/" + dashboardKey;
}

/**
* Create a new LoggedTunableNumber with the default value
*
* @param dashboardKey Key on dashboard
* @param defaultValue Default value
*/
public LoggedTunableNumber(String dashboardKey, double defaultValue) {
this(dashboardKey);
initDefault(defaultValue);
}

/**
* Set the default value of the number. The default value can only be set once.
*
* @param defaultValue The default value
*/
public void initDefault(double defaultValue) {
if (!hasDefault) {
hasDefault = true;
this.defaultValue = defaultValue;
if (Constants.tuningMode) {
dashboardNumber = new LoggedNetworkNumber(key, defaultValue);
}
}
}

/**
* Get the current value, from dashboard if available and in tuning mode.
*
* @return The current value
*/
public double get() {
if (!hasDefault) {
return 0.0;
} else {
return Constants.tuningMode ? dashboardNumber.get() : defaultValue;
}
}

/**
* Checks whether the number has changed since our last check
*
* @param id Unique identifier for the caller to avoid conflicts when shared between multiple
* objects. Recommended approach is to pass the result of "hashCode()"
* @return True if the number has changed since the last time this method was called, false
* otherwise.
*/
public boolean hasChanged(int id) {
double currentValue = get();
Double lastValue = lastHasChangedValues.get(id);
if (lastValue == null || currentValue != lastValue) {
lastHasChangedValues.put(id, currentValue);
return true;
}

return false;
}

/**
* Runs action if any of the tunableNumbers have changed
*
* @param id Unique identifier for the caller to avoid conflicts when shared between multiple *
* objects. Recommended approach is to pass the result of "hashCode()"
* @param action Callback to run when any of the tunable numbers have changed. Access tunable
* numbers in order inputted in method
* @param tunableNumbers All tunable numbers to check
*/
public static void ifChanged(
int id, Consumer<double[]> action, LoggedTunableNumber... tunableNumbers) {
if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) {
action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray());
}
}

/** Runs action if any of the tunableNumbers have changed */
public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) {
ifChanged(id, values -> action.run(), tunableNumbers);
}

@Override
public double getAsDouble() {
return get();
}
}

0 comments on commit 7dc28f2

Please sign in to comment.