diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f5a5839..43afa60 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -59,10 +59,13 @@ public enum Mode { public static final double TILT_GEARING = 45.0; public static final boolean TILT_SIMULATE_GRAVITY = false; + public static final boolean TUNING_MODE = false; public static final boolean ARM_DEBUG = true; public static final double DRIVE_BASE_WIDTH = 0.635; public static final double DRIVE_BASE_LENGTH = 0.635; public static final double INITIAL_ROBOT_HEIGHT = 0; + public static final double GRAVITY = 9.81; + } diff --git a/src/main/java/frc/robot/utils/diag/DiagBoolean.java b/src/main/java/frc/robot/utils/diag/DiagBoolean.java new file mode 100644 index 0000000..4c0b655 --- /dev/null +++ b/src/main/java/frc/robot/utils/diag/DiagBoolean.java @@ -0,0 +1,68 @@ +package frc.robot.utils.diag; + +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; + +/** + * Abstract diagnostics class for boolean sensors using DigitalInput The diagnostics will turn green + * once the switch has changes to reflect both positions: ON and OFF + */ +public abstract class DiagBoolean implements Diagnosable { + + private String title; + private String name; + private GenericEntry networkTableEntry; + + private boolean seenFalse; + private boolean seenTrue; + + /** + * Constructor + * + * @param name the name of the unit. Will be used on the Shuffleboard + */ + public DiagBoolean(String title, String name) { + this.title = title; + this.name = name; + + reset(); + } + + @Override + public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int height) { + networkTableEntry = + shuffleBoardTab + .getLayout(title, BuiltInLayouts.kList) + .withSize(width, height) + .add(name, false) + .getEntry(); + } + + @Override + public void refresh() { + if (networkTableEntry != null) { + networkTableEntry.setBoolean(getDiagResult()); + } + } + + @Override + public void reset() { + seenFalse = seenTrue = false; + } + + protected abstract boolean getValue(); + + // Package protected + boolean getDiagResult() { + boolean currentValue = getValue(); + // Set the value for the state - whether the switch is pressed or not + if (currentValue) { + seenTrue = true; + } else { + seenFalse = true; + } + + return seenTrue && seenFalse; + } +} diff --git a/src/main/java/frc/robot/utils/diag/DiagDistanceTraveled.java b/src/main/java/frc/robot/utils/diag/DiagDistanceTraveled.java new file mode 100644 index 0000000..68e6922 --- /dev/null +++ b/src/main/java/frc/robot/utils/diag/DiagDistanceTraveled.java @@ -0,0 +1,60 @@ +package frc.robot.utils.diag; + +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; + +/** + * Base class for Diagnosable that have an initial value (their "current" value) and that are + * required to change their value by a certain amount from that + */ +public abstract class DiagDistanceTraveled implements Diagnosable { + + protected String name; + protected String title; + protected double requiredTravel; + protected GenericEntry networkTableEntry; + private double initialValue; + private boolean traveledDistance; + + public DiagDistanceTraveled(String title, String name, double requiredTravel) { + this.title = title; + this.name = name; + this.requiredTravel = requiredTravel; + } + + @Override + public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int height) { + networkTableEntry = + shuffleBoardTab + .getLayout(title, BuiltInLayouts.kList) + .withSize(width, height) + .add(name, false) + .getEntry(); + } + + @Override + public void refresh() { + if (networkTableEntry != null) { + networkTableEntry.setBoolean(getDiagResult()); + } + } + + @Override + public void reset() { + traveledDistance = false; + initialValue = getCurrentValue(); + } + + boolean getDiagResult() { + double currentValue = getCurrentValue(); + + if (Math.abs(currentValue - initialValue) >= requiredTravel) { + traveledDistance = true; + } + + return this.traveledDistance; + } + + protected abstract double getCurrentValue(); +} diff --git a/src/main/java/frc/robot/utils/diag/DiagDutyCycleEncoder.java b/src/main/java/frc/robot/utils/diag/DiagDutyCycleEncoder.java new file mode 100644 index 0000000..4731372 --- /dev/null +++ b/src/main/java/frc/robot/utils/diag/DiagDutyCycleEncoder.java @@ -0,0 +1,33 @@ +package frc.robot.utils.diag; + +import edu.wpi.first.wpilibj.DutyCycleEncoder; + +/** + * A diagnostics class for digital encoder. The diagnostics will turn green once the encoder has + * traveled at least a given distance from its initial position (measured at initialization or after + * a reset) + */ +public class DiagDutyCycleEncoder extends DiagDistanceTraveled { + + private DutyCycleEncoder encoder; + + /** + * Constructor + * + * @param name - the name of the unit. Will be used on the Shuffleboard + * @param requiredTravel - the required difference between the initial position to qualify for + * success + * @param encoder - the encoder instance to test + */ + public DiagDutyCycleEncoder( + String title, String name, double requiredTravel, DutyCycleEncoder encoder) { + super(title, name, requiredTravel); + this.encoder = encoder; + reset(); + } + + @Override + protected double getCurrentValue() { + return encoder.get(); + } +} diff --git a/src/main/java/frc/robot/utils/diag/DiagEncoder.java b/src/main/java/frc/robot/utils/diag/DiagEncoder.java new file mode 100644 index 0000000..3c34273 --- /dev/null +++ b/src/main/java/frc/robot/utils/diag/DiagEncoder.java @@ -0,0 +1,32 @@ +package frc.robot.utils.diag; + +import edu.wpi.first.wpilibj.Encoder; + +/** + * A diagnostics class for digital encoder. The diagnostics will turn green once the encoder has + * traveled at least a given distance from its initial position (measured at initialization or after + * a reset) + */ +public class DiagEncoder extends DiagDistanceTraveled { + + private Encoder encoder; + + /** + * Constructor + * + * @param name - the name of the unit. Will be used on the Shuffleboard + * @param requiredTravel - the required difference between the initial position to qualify for + * success + * @param encoder - the encoder instance to test + */ + public DiagEncoder(String title, String name, double requiredTravel, Encoder encoder) { + super(title, name, requiredTravel); + this.encoder = encoder; + reset(); + } + + @Override + protected double getCurrentValue() { + return encoder.get(); + } +} diff --git a/src/main/java/frc/robot/utils/diag/DiagGyro.java b/src/main/java/frc/robot/utils/diag/DiagGyro.java new file mode 100644 index 0000000..002e40a --- /dev/null +++ b/src/main/java/frc/robot/utils/diag/DiagGyro.java @@ -0,0 +1,18 @@ +package frc.robot.utils.diag; + +import frc.robot.utils.logging.input.GyroValues; +import frc.robot.utils.logging.io.gyro.ThreadedGyro; + +public class DiagGyro extends DiagDistanceTraveled { + private final ThreadedGyro gyro; + + public DiagGyro(String title, String name, double requiredTravel, ThreadedGyro gyro) { + super(title, name, requiredTravel); + this.gyro = gyro; + } +//broken currently fix return + @Override + protected double getCurrentValue() { + return 0; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/diag/DiagLimelight.java b/src/main/java/frc/robot/utils/diag/DiagLimelight.java new file mode 100644 index 0000000..ad4fbdd --- /dev/null +++ b/src/main/java/frc/robot/utils/diag/DiagLimelight.java @@ -0,0 +1,47 @@ +package frc.robot.utils.diag; + +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; + +public class DiagLimelight implements Diagnosable { + private DoubleSubscriber subscriber; + private String title; + private String name; + private GenericEntry networkTableEntry; + + public DiagLimelight(String title, String name) { + this.title = title; + this.name = name; + reset(); + } + + protected boolean getDiagResult() { + return subscriber.get() == 1; + } + + @Override + public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int height) { + networkTableEntry = + shuffleBoardTab + .getLayout(title, BuiltInLayouts.kList) + .withSize(width, height) + .add(name, false) + .getEntry(); + } + + @Override + public void refresh() { + if (networkTableEntry != null) { + networkTableEntry.setBoolean(getDiagResult()); + } + } + + @Override + public void reset() { + subscriber = + NetworkTableInstance.getDefault().getTable("limelight").getDoubleTopic("tv").subscribe(-1); + } +} diff --git a/src/main/java/frc/robot/utils/diag/DiagMinMax.java b/src/main/java/frc/robot/utils/diag/DiagMinMax.java new file mode 100644 index 0000000..63e9c25 --- /dev/null +++ b/src/main/java/frc/robot/utils/diag/DiagMinMax.java @@ -0,0 +1,71 @@ +package frc.robot.utils.diag; + +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; + +/** + * Abstract diagnostics class for devices that need a minimum and maximum reading to be tested. The + * diagnostics will turn green once the device has reached both points. + */ +public abstract class DiagMinMax implements Diagnosable { + + private String name; + private String title; + private double minValue; + private double maxValue; + private GenericEntry networkTableEntry; + + private boolean seenMinValue; + private boolean seenMaxValue; + + /** + * Constructor + * + * @param name - the name of the unit. Will be used on the Shuffleboard + * @param minValue - the minimum value the object needs to hit to qualify for success + * @param maxValue - the maximum value the object needs to hit to qualify for success + */ + public DiagMinMax(String title, String name, double minValue, double maxValue) { + this.name = name; + this.title = title; + this.minValue = minValue; + this.maxValue = maxValue; + + reset(); + } + + @Override + public void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int height) { + networkTableEntry = + shuffleBoardTab + .getLayout(title, BuiltInLayouts.kList) + .withSize(width, height) + .add(name, false) + .getEntry(); + } + + @Override + public void refresh() { + if (networkTableEntry != null) { + networkTableEntry.setBoolean(getDiagResult(getSensorReading())); + } + } + + @Override + public void reset() { + seenMinValue = seenMaxValue = false; + } + + abstract double getSensorReading(); + + boolean getDiagResult(double value) { + if (value >= maxValue) { + seenMaxValue = true; + } else if (value <= minValue) { + seenMinValue = true; + } + + return seenMaxValue && seenMinValue; + } +} diff --git a/src/main/java/frc/robot/utils/diag/DiagPot.java b/src/main/java/frc/robot/utils/diag/DiagPot.java new file mode 100644 index 0000000..e596d85 --- /dev/null +++ b/src/main/java/frc/robot/utils/diag/DiagPot.java @@ -0,0 +1,31 @@ +package frc.robot.utils.diag; + +import edu.wpi.first.wpilibj.AnalogPotentiometer; + +/** + * A diagnostics class for analog potentiometer. It is a DiagMinMax object. Give it the maximum and + * minimum voltages for testing with. + */ +public class DiagPot extends DiagMinMax { + + private AnalogPotentiometer pot; + + /** + * Constructor + * + * @param name - the name of the unit. Will be used on the Shuffleboard + * @param minVoltage - the minimum value the pot needs to hit to qualify for success + * @param maxVoltage - the maximum value the pot needs to hit to qualify for success + * @param pot - the pot instance to test + */ + public DiagPot( + String title, String name, double minVoltage, double maxVoltage, AnalogPotentiometer pot) { + super(title, name, minVoltage, maxVoltage); + this.pot = pot; + } + + @Override + double getSensorReading() { + return pot.get(); + } +} diff --git a/src/main/java/frc/robot/utils/diag/DiagSparkMaxAbsEncoder.java b/src/main/java/frc/robot/utils/diag/DiagSparkMaxAbsEncoder.java new file mode 100644 index 0000000..89f07a8 --- /dev/null +++ b/src/main/java/frc/robot/utils/diag/DiagSparkMaxAbsEncoder.java @@ -0,0 +1,33 @@ +package frc.robot.utils.diag; + +import com.ctre.phoenix6.hardware.CANcoder; + +/** + * A diagnostics class for digital encoder. The diagnostics will turn green once the encoder has + * traveled at least a given distance from its initial position (measured at initialization or after + * a reset) + */ +public class DiagSparkMaxAbsEncoder extends DiagDistanceTraveled { + + private CANcoder canCoder; + + /** + * Constructor + * + * @param name - the name of the unit. Will be used on the Shuffleboard + * @param requiredTravel - the required difference between the initial position to qualify for + * success + * @param canSparkMax - the encoder instance to test + */ + public DiagSparkMaxAbsEncoder( + String title, String name, double requiredTravel, CANcoder canCoder) { + super(title, name, requiredTravel); + this.canCoder = canCoder; + reset(); + } +//Change returns from 2025 + @Override + protected double getCurrentValue() { + return canCoder.getPosition().getValueAsDouble(); + } +} diff --git a/src/main/java/frc/robot/utils/diag/DiagSparkMaxEncoder.java b/src/main/java/frc/robot/utils/diag/DiagSparkMaxEncoder.java new file mode 100644 index 0000000..469f5fc --- /dev/null +++ b/src/main/java/frc/robot/utils/diag/DiagSparkMaxEncoder.java @@ -0,0 +1,33 @@ +package frc.robot.utils.diag; + +import com.revrobotics.spark.SparkMax; + +/** + * A diagnostics class for digital encoder. The diagnostics will turn green once the encoder has + * traveled at least a given distance from its initial position (measured at initialization or after + * a reset) + */ +public class DiagSparkMaxEncoder extends DiagDistanceTraveled { + + private SparkMax canSparkMax; + + /** + * Constructor + * + * @param name - the name of the unit. Will be used on the Shuffleboard + * @param requiredTravel - the required difference between the initial position to qualify for + * success + * @param canSparkMax - the encoder instance to test + */ + public DiagSparkMaxEncoder( + String title, String name, double requiredTravel, SparkMax canSparkMax) { + super(title, name, requiredTravel); + this.canSparkMax = canSparkMax; + reset(); + } + + @Override + protected double getCurrentValue() { + return canSparkMax.getEncoder().getPosition(); + } +} diff --git a/src/main/java/frc/robot/utils/diag/DiagSparkMaxSwitch.java b/src/main/java/frc/robot/utils/diag/DiagSparkMaxSwitch.java new file mode 100644 index 0000000..27240d0 --- /dev/null +++ b/src/main/java/frc/robot/utils/diag/DiagSparkMaxSwitch.java @@ -0,0 +1,46 @@ +package frc.robot.utils.diag; + +import com.revrobotics.spark.SparkMax; + +/** + * Diagnostics class for digital switch connected directly to the talon SRX it is a DiagBoolean + * subclass. + */ +public class DiagSparkMaxSwitch extends DiagBoolean { + public DiagSparkMaxSwitch(String title, String name) { + super(title, name); + } + + public enum Direction { + FORWARD, + REVERSE + }; + + private SparkMax canSparkMax; + private Direction direction; + + /* + * Constructor + * + * @param name -the name of the unit. Will be used on the Shuffleboard + * @param talonSRX -the talon SRX to read the switch value from + */ + + public DiagSparkMaxSwitch(String title, String name, SparkMax canSparkMax, Direction direction) { + super(title, name); + this.canSparkMax = canSparkMax; + this.direction = direction; + } + + @Override + protected boolean getValue() { + switch (direction) { + case FORWARD: + return canSparkMax.getForwardLimitSwitch().isPressed(); + case REVERSE: + return canSparkMax.getReverseLimitSwitch().isPressed(); + default: + return false; + } + } +} diff --git a/src/main/java/frc/robot/utils/diag/DiagSwitch.java b/src/main/java/frc/robot/utils/diag/DiagSwitch.java new file mode 100644 index 0000000..ab7afcd --- /dev/null +++ b/src/main/java/frc/robot/utils/diag/DiagSwitch.java @@ -0,0 +1,25 @@ +package frc.robot.utils.diag; + +import edu.wpi.first.wpilibj.DigitalInput; + +/** Diagnostics class for digital switch; it is a DiagBoolean object. */ +public class DiagSwitch extends DiagBoolean { + + private DigitalInput digitalInput; + + /** + * Constructor + * + * @param name the name of the unit. Will be used on the Shuffleboard + * @param digitalInput - the DigitalInput the switch is connected to + */ + public DiagSwitch(String title, String name, DigitalInput digitalInput) { + super(title, name); + this.digitalInput = digitalInput; + } + + @Override + protected boolean getValue() { + return digitalInput.get(); + } +} diff --git a/src/main/java/frc/robot/utils/diag/Diagnosable.java b/src/main/java/frc/robot/utils/diag/Diagnosable.java new file mode 100644 index 0000000..526a50b --- /dev/null +++ b/src/main/java/frc/robot/utils/diag/Diagnosable.java @@ -0,0 +1,19 @@ +package frc.robot.utils.diag; + +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; + +public interface Diagnosable { + + /** + * Set the tab for the diagnosable (this is done by the diagnostics infrastructure) + * + * @param shuffleBoardTab the tab to set + */ + void setShuffleBoardTab(ShuffleboardTab shuffleBoardTab, int width, int height); + + /** A method called periodically that will test the diagnosable value and update the display */ + public void refresh(); + + /** A method to reset the "tested" state of the diagnosable component to its initial state */ + public void reset(); +} diff --git a/src/main/java/frc/robot/utils/diag/Diagnostics.java b/src/main/java/frc/robot/utils/diag/Diagnostics.java new file mode 100644 index 0000000..3d82f6d --- /dev/null +++ b/src/main/java/frc/robot/utils/diag/Diagnostics.java @@ -0,0 +1,64 @@ +package frc.robot.utils.diag; + +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; +import frc.robot.RobotMode; + +import java.util.ArrayList; +import java.util.List; + +/** + * The subsystem that handles diagnostics. This subsystem will hold the list of Diagnosables and + * make sure they go through their lifecycle: refresh and reset. Use addDiagnosable() to add new + * components to the list - NOTE: Make sure they are only added to the list ONCE in the Robot's + * lifetime (e.g. in the subsystem constructor). + */ +public class Diagnostics extends SubsystemBase { + + public static final String SHUFFLEBOARD_TAB_NAME = "Diagnostics"; + public static final int width = 1, height = 4; + + private ShuffleboardTab shuffleBoardTab; + + private List diagnosables; + + public Diagnostics() { + shuffleBoardTab = Shuffleboard.getTab(SHUFFLEBOARD_TAB_NAME); + diagnosables = new ArrayList<>(); + + // This simulates adding components by other subsystems... + /* + addDiagnosable(new DiagSwitch("Switch1", new DigitalInput(1), shuffleBoardTab)); + addDiagnosable(new DiagPot("Pot1", 0.1, 0.9, new AnalogPotentiometer(0), shuffleBoardTab)); + addDiagnosable(new DiagEncoder("Encoder1", 100, new Encoder(5,6), shuffleBoardTab)); + addDiagnosable(new DiagOpticalSensor("OpticalSensor1", new DigitalInput(2), shuffleBoardTab)); + addDiagnosable(new DiagOpticalRangeFinder("OpticalRangeFinder1", new OpticalRangeFinder(new AnalogInput(1)), shuffleBoardTab, 3.0, 12.0)); + addDiagnosable(new DiagSonar("Sonar1", new Ultrasonic(3, 4), shuffleBoardTab, 3.0, 12.0)); + */ + } + + public void addDiagnosable(Diagnosable diagnosable) { + diagnosable.setShuffleBoardTab(shuffleBoardTab, width, height); + diagnosables.add(diagnosable); + } + + /** + * Refresh the display with current values. This would go through the range of components and test + * their state, and then refresh the shuffleboard display This method should be called + * periodically (e.g. in test mode) to ensure the components are all tested + */ + public void refresh() { + if (Robot.getMode() == RobotMode.TEST) diagnosables.forEach(Diagnosable::refresh); + } + + /** + * Reset the diagnostics to the initial values. This would have the effect of getting all the + * diagnosable items to their initial state (as if they were not tested yet) This can be done when + * disabling the robot, so enable->disable would allow for a retest + */ + public void reset() { + diagnosables.forEach(Diagnosable::reset); + } +} diff --git a/src/main/java/frc/robot/utils/logging/LoggedTunableNumber.java b/src/main/java/frc/robot/utils/logging/LoggedTunableNumber.java new file mode 100644 index 0000000..ee741e7 --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/LoggedTunableNumber.java @@ -0,0 +1,125 @@ +// 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. +// Modified by FRC 4048 + +package frc.robot.utils.logging; + +import java.util.Arrays; +import java.util.HashMap; +import java.util.Map; +import java.util.function.Consumer; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + +import frc.robot.Constants; + +/** + * 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 boolean hasDefault = false; + private double defaultValue; + private LoggedNetworkNumber dashboardNumber; + private final Map lastHasChangedValues = new HashMap<>(); + private final String key; + + /** + * 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.TUNING_MODE) { + 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.TUNING_MODE ? 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 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(); + } +} diff --git a/src/main/java/frc/robot/utils/logging/PIDIo.java b/src/main/java/frc/robot/utils/logging/PIDIo.java new file mode 100644 index 0000000..66e0d37 --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/PIDIo.java @@ -0,0 +1,9 @@ +package frc.robot.utils.logging; + +import frc.robot.utils.logging.input.PidMotorInputs; +import frc.robot.utils.motor.NeoPidConfig; +import frc.robot.utils.logging.io.BaseIoImpl; + +public interface PIDIo { + void configurePID(NeoPidConfig pidConfig); +} \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/logging/PIDLoggableIO.java b/src/main/java/frc/robot/utils/logging/PIDLoggableIO.java new file mode 100644 index 0000000..7050e65 --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/PIDLoggableIO.java @@ -0,0 +1,8 @@ +/*package frc.robot.utils.logging; + +import frc.robot.utils.logging.commands.Loggable; +import frc.robot.utils.motor.NeoPidConfig; + +public interface PIDLoggableIO extends Loggable { + void configurePID(NeoPidConfig pidConfig); +}*/ diff --git a/src/main/java/frc/robot/utils/logging/input/PidMotorInputs.java b/src/main/java/frc/robot/utils/logging/input/PidMotorInputs.java new file mode 100644 index 0000000..bc90445 --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/input/PidMotorInputs.java @@ -0,0 +1,174 @@ +package frc.robot.utils.logging.input; + +import com.revrobotics.spark.SparkMax; + +import frc.robot.utils.motor.NeoPidConfig; +import frc.robot.utils.motor.NeoPidMotor; + +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +import java.util.EnumSet; +import java.util.Objects; + +/** + * A {@link org.littletonrobotics.junction.inputs.LoggableInputs} subclass that can handle motor state. + * This implementation can hold state for a generic motor controller and be updated from various types + * of "real" motor controllers. + */ +public class PidMotorInputs implements LoggableInputs { + public enum PIDMotorInputTypes { + ENCODER_POSITION("encoderPosition"), + ENCODER_VELOCITY("encoderVelocity"), + MOTOR_CURRENT("motorCurrent"), + MOTOR_TEMPERATURE("motorTemperature"), + APPLIED_OUTPUT("appliedOutput"), + FWD_LIMIT_SWITCH("fwdLimit"), + REV_LIMIT_SWITCH("revLimit"), + PID_SETPOINT("pidSetpoint"); + + private final String logKey; + + PIDMotorInputTypes(String logKey) { + this.logKey = logKey; + } + + public String getLogKey() { + return logKey; + } + } + + // Set of booleans for values that we care about + private final EnumSet loggedInputFilter; + private double encoderPosition = 0.0; + private double encoderVelocity = 0.0; + private double motorCurrent = 0.0; + private double motorTemperature = 0.0; + private double appliedOutput = 0.0; + private boolean fwdLimit = false; + private boolean revLimit = false; + private double pidSetpoint = 0; + + public PidMotorInputs(EnumSet loggedInputFilter) { + this.loggedInputFilter = Objects.requireNonNull(loggedInputFilter); + } + + public static PidMotorInputs allMetrics() { + return new PidMotorInputs(EnumSet.allOf(PIDMotorInputTypes.class)); + } + + @Override + public void toLog(LogTable table) { + if (loggedInputFilter.contains(PIDMotorInputTypes.ENCODER_POSITION)) { + table.put(PIDMotorInputTypes.ENCODER_POSITION.getLogKey(), encoderPosition); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.ENCODER_VELOCITY)) { + table.put(PIDMotorInputTypes.ENCODER_VELOCITY.getLogKey(), encoderVelocity); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.MOTOR_CURRENT)) { + table.put(PIDMotorInputTypes.MOTOR_CURRENT.getLogKey(), motorCurrent); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.MOTOR_TEMPERATURE)) { + table.put(PIDMotorInputTypes.MOTOR_TEMPERATURE.getLogKey(), motorTemperature); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.APPLIED_OUTPUT)) { + table.put(PIDMotorInputTypes.APPLIED_OUTPUT.getLogKey(), appliedOutput); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.FWD_LIMIT_SWITCH)) { + table.put(PIDMotorInputTypes.FWD_LIMIT_SWITCH.getLogKey(), fwdLimit); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.REV_LIMIT_SWITCH)) { + table.put(PIDMotorInputTypes.REV_LIMIT_SWITCH.getLogKey(), revLimit); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.PID_SETPOINT)) { + table.put(PIDMotorInputTypes.PID_SETPOINT.getLogKey(), pidSetpoint); + } + } + + @Override + public void fromLog(LogTable table) { + if (loggedInputFilter.contains(PIDMotorInputTypes.ENCODER_POSITION)) { + encoderPosition = table.get(PIDMotorInputTypes.ENCODER_POSITION.getLogKey(), encoderPosition); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.ENCODER_VELOCITY)) { + encoderVelocity = table.get(PIDMotorInputTypes.ENCODER_VELOCITY.getLogKey(), encoderVelocity); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.MOTOR_CURRENT)) { + motorCurrent = table.get(PIDMotorInputTypes.MOTOR_CURRENT.getLogKey(), motorCurrent); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.MOTOR_TEMPERATURE)) { + motorTemperature = table.get(PIDMotorInputTypes.MOTOR_TEMPERATURE.getLogKey(), motorTemperature); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.APPLIED_OUTPUT)) { + appliedOutput = table.get(PIDMotorInputTypes.APPLIED_OUTPUT.getLogKey(), appliedOutput); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.FWD_LIMIT_SWITCH)) { + fwdLimit = table.get(PIDMotorInputTypes.FWD_LIMIT_SWITCH.getLogKey(), fwdLimit); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.REV_LIMIT_SWITCH)) { + revLimit = table.get(PIDMotorInputTypes.REV_LIMIT_SWITCH.getLogKey(), revLimit); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.PID_SETPOINT)) { + pidSetpoint = table.get(PIDMotorInputTypes.PID_SETPOINT.getLogKey(), pidSetpoint); + } + } + + public void fromHardware(NeoPidMotor pidMotor) { + SparkMax sparkMax = pidMotor.getNeoMotor(); + if (loggedInputFilter.contains(PIDMotorInputTypes.ENCODER_POSITION)) { + encoderPosition = sparkMax.getEncoder().getPosition(); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.ENCODER_VELOCITY)) { + encoderVelocity = sparkMax.getEncoder().getVelocity(); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.MOTOR_CURRENT)) { + motorCurrent = sparkMax.getOutputCurrent(); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.MOTOR_TEMPERATURE)) { + motorTemperature = sparkMax.getMotorTemperature(); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.APPLIED_OUTPUT)) { + appliedOutput = sparkMax.getAppliedOutput(); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.FWD_LIMIT_SWITCH)) { + fwdLimit = sparkMax.getForwardLimitSwitch().isPressed(); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.REV_LIMIT_SWITCH)) { + revLimit = sparkMax.getReverseLimitSwitch().isPressed(); + } + if (loggedInputFilter.contains(PIDMotorInputTypes.PID_SETPOINT)) { + pidMotor.getPidPosition(); + } + } + + public Double getEncoderPosition() { + return encoderPosition; + } + + public Double getEncoderVelocity() { + return encoderVelocity; + } + + public Double getMotorCurrent() { + return motorCurrent; + } + + public Double getMotorTemperature() { + return motorTemperature; + } + + public Boolean getFwdLimit() { + return fwdLimit; + } + + public Boolean getRevLimit() { + return revLimit; + } + + public Double getAppliedOutput() { + return appliedOutput; + } + public Double getPidPosition() { + return pidSetpoint; + } +} diff --git a/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java b/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java index 56479e3..3438f22 100644 --- a/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java +++ b/src/main/java/frc/robot/utils/logging/io/motor/RealSparkMaxIo.java @@ -48,4 +48,8 @@ public boolean isRevSwitchPressed() { protected void updateInputs(MotorLoggableInputs inputs) { inputs.fromHardware(motor); } + + public SparkMax getSparkMax() { + return motor; + } } diff --git a/src/main/java/frc/robot/utils/math/AngleUtils.java b/src/main/java/frc/robot/utils/math/AngleUtils.java new file mode 100644 index 0000000..d9fb9f5 --- /dev/null +++ b/src/main/java/frc/robot/utils/math/AngleUtils.java @@ -0,0 +1,50 @@ +package frc.robot.utils.math; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class AngleUtils { + /** + * This is a simple method that subtracts the given angle from PI/2 + * + * @param angle to find complement of + * @return compliment of angle + */ + public static Rotation2d compliment(Rotation2d angle) { + return new Rotation2d(Math.PI / 2).minus(angle); + } + + /** + * Reduces angle to be between 0 and 2*PI + * + * @param angle to reduce + * @return reduce angle + */ + public static Rotation2d reduce(Rotation2d angle) { + return new Rotation2d((angle.getRadians() % (2 * Math.PI))); + } + + /** + * Converts the angle to be between -PI/2 and PI/2. The function works by passing the sin of the + * angle to the arcSin function resulting in an angle clamped between -PI/2 and PI/2 because of + * the range of the arcSin function + * + * @param angle to normalize + * @return normalized angle + */ + public static Rotation2d normalize(Rotation2d angle) { + return new Rotation2d( + Math.asin(Math.sin(angle.getRadians()))); // can be simplified (reduce(angle+PI) - PI + } + + public static double normalizeSwerveAngle(double angleInRad) { + angleInRad %= 2 * Math.PI; + if (angleInRad < 0) { + angleInRad += 2 * Math.PI; + } + return angleInRad; + } + + public static double normalizeAbsAngleRadians(double angleInRev) { + return angleInRev * 2 * Math.PI; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/math/ArrayUtils.java b/src/main/java/frc/robot/utils/math/ArrayUtils.java new file mode 100644 index 0000000..d7b07f0 --- /dev/null +++ b/src/main/java/frc/robot/utils/math/ArrayUtils.java @@ -0,0 +1,80 @@ + +package frc.robot.utils.math; + +import java.lang.reflect.Array; +import java.util.Arrays; +import java.util.List; + +public class ArrayUtils { + public static boolean contains(T[] array, T value) { + for (T t : array) { + if (t.equals(value)) { + return true; + } + } + return false; + } + + public static boolean contains(double[] array, double value) { + for (double d : array) { + if (d == value) { + return true; + } + } + return false; + } + + public static boolean contains(int[] array, int value) { + for (int d : array) { + if (d == value) { + return true; + } + } + return false; + } + + public static boolean allMatch(double[] array, double value) { + for (double d : array) { + if (d != value) { + return false; + } + } + return true; + } + + public static boolean allMatch(int[] array, int value) { + for (int d : array) { + if (d != value) { + return false; + } + } + return true; + } + + public static boolean allMatch(T[] array, T value) { + for (T t : array) { + if (!t.equals(value)) { + return false; + } + } + return true; + } + + @SuppressWarnings("unchecked") + public static T[] toArray(List list, Class tClass) { + T[] t = (T[]) Array.newInstance(tClass, list.size()); + return list.toArray(t); + } + + public static double[] unwrap(Double[] a) { + return Arrays.stream(a).mapToDouble(Double::doubleValue).toArray(); + } + + public static int[] unwrap(Integer[] a) { + return Arrays.stream(a).mapToInt(Integer::intValue).toArray(); + } + + public static long[] unwrap(Long[] a) { + return Arrays.stream(a).mapToLong(Long::longValue).toArray(); + } +} diff --git a/src/main/java/frc/robot/utils/math/PoseUtils.java b/src/main/java/frc/robot/utils/math/PoseUtils.java new file mode 100644 index 0000000..3ad69ca --- /dev/null +++ b/src/main/java/frc/robot/utils/math/PoseUtils.java @@ -0,0 +1,221 @@ +package frc.robot.utils.math; + +import edu.wpi.first.math.geometry.*; + +public class PoseUtils { + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Pose2d getFieldEstimatedFuturePose(Pose2d pose, double vx, double vy, double time) { + return getFieldEstimatedFuturePose(pose, vx, vy, 0.0, time); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Transform2d getFieldEstimatedFuturePose( + Transform2d pose, double vx, double vy, double time) { + return getFieldEstimatedFuturePose(pose, vx, vy, 0.0, time); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Translation2d getFieldEstimatedFuturePose( + Translation2d pose, double vx, double vy, double time) { + return pose.plus(new Translation2d(vx * time, vy * time)); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Pose3d getFieldEstimatedFuturePose( + Pose3d pose, double vx, double vy, double vz, double time) { + return getFieldEstimatedFuturePose(pose, vx, vy, vz, 0.0, 0.0, 0.0, time); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Transform3d getFieldEstimatedFuturePose( + Transform3d pose, double vx, double vy, double vz, double time) { + return getFieldEstimatedFuturePose(pose, vx, vy, vz, 0.0, 0.0, 0.0, time); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Translation3d getFieldEstimatedFuturePose( + Translation3d pose, double vx, double vy, double vz, double time) { + return pose.plus(new Translation3d(vx * time, vy * time, vz * time)); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vYaw yaw velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Pose2d getFieldEstimatedFuturePose( + Pose2d pose, double vx, double vy, double vYaw, double time) { + return pose.transformBy(new Transform2d(vx * time, vy * time, new Rotation2d(vYaw * time))); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vYaw yaw velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Transform2d getFieldEstimatedFuturePose( + Transform2d pose, double vx, double vy, double vYaw, double time) { + return pose.plus(new Transform2d(vx * time, vy * time, Rotation2d.fromDegrees(vYaw * time))); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param vYaw yaw velocity + * @param vRoll roll velocity + * @param vPitch pitch velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Pose3d getFieldEstimatedFuturePose( + Pose3d pose, + double vx, + double vy, + double vz, + double vYaw, + double vRoll, + double vPitch, + double time) { + return pose.transformBy( + new Transform3d( + vx * time, + vy * time, + vz * time, + new Rotation3d(vRoll * time, vPitch * time, vYaw * time))); + } + + /** + * Estimates a future position based on the current position and velocities + * + * @param pose current pose + * @param vx x velocity + * @param vy y velocity + * @param vz z velocity + * @param vYaw yaw velocity + * @param vRoll roll velocity + * @param vPitch pitch velocity + * @param time time over which velocity is applied + * @return Estimated position + */ + public static Transform3d getFieldEstimatedFuturePose( + Transform3d pose, + double vx, + double vy, + double vz, + double vYaw, + double vRoll, + double vPitch, + double time) { + return pose.plus( + new Transform3d( + vx * time, + vy * time, + vz * time, + new Rotation3d(vRoll * time, vPitch * time, vYaw * time))); + } + + /** + * @param pose to modify + * @param z to add + * @param pitch to add + * @param roll to add + * @return 3D pose that inherits 2D attributes + */ + public static Pose3d addDimension(Pose2d pose, double z, double pitch, double roll) { + return new Pose3d( + addDimension(pose.getTranslation(), z), addDimension(pose.getRotation(), pitch, roll)); + } + + /** + * @param pose to modify + * @param z to add + * @return 3D pose that inherits 2D attributes + */ + public static Translation3d addDimension(Translation2d pose, double z) { + return new Translation3d(pose.getX(), pose.getY(), z); + } + + /** + * @param rotation to modify + * @param pitch to add + * @param roll to add + * @return 3D rotation that inherits yaw of 2D rotation + */ + public static Rotation3d addDimension(Rotation2d rotation, double pitch, double roll) { + return new Rotation3d(roll, pitch, rotation.getRadians()); + } + + public static double slope(Translation2d startPose, Translation2d endPose) { + return (endPose.getY() - startPose.getY()) / (endPose.getX() - startPose.getX()); + } + + public static double getDistance(Pose2d pose1, Pose2d pose2) { + return Math.sqrt( + Math.pow(pose1.getX() - pose2.getX(), 2) + Math.pow(pose1.getY() - pose2.getY(), 2)); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/math/VectorUtils.java b/src/main/java/frc/robot/utils/math/VectorUtils.java new file mode 100644 index 0000000..471612e --- /dev/null +++ b/src/main/java/frc/robot/utils/math/VectorUtils.java @@ -0,0 +1,283 @@ +package frc.robot.utils.math; + +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.Constants; +import org.littletonrobotics.junction.Logger; + +/** Utility class for vectors used in projectile motion */ +public class VectorUtils { + + /** + * The projectile motion converts the given coords to 2d after applying the change in base coords + * from the arc provided. Because shooter angle depends on position (not point math) and position + * depends on angle (not point math), the function will approximate to a specified certainty by + * calling the function at 45 degrees, getting that pose and then looping until the delta angle + * result is within a specified margin, or you have exceeded the max iterations specified. + * + * @param speed the initial velocity of the projectile + * @param centerArcX the x cord of the arc created by the shooter moving at different angles + * @param centerArcY the y cord of the arc created by the shooter moving at different angles + * @param centerArcZ the z cord of the arc created by the shooter moving at different angles + * @param arcRadius the length of the shooter, which forms an arc when changing angle + * @param destX the target position in the x direction + * @param destY the target position in the y direction + * @param destZ the target position in the z direction + * @param degreeThreshold threshold before angles are considered equal when iterating over + * positions angle relationship created by arc + * @param maxIterations the max amount of iterations before an angle is returned + * @param maxFractionalRange double representing what fraction of range of the parabola is + * acceptable for hitting your target.
+ * * For example if you have to hit your target from bottom, you can constrain the parabola to + * only be acceptable in the first third of the parabola. + * @return a {@link VelocityVector} with the calculated target angle between 0 and 90 degrees.
+ * Impossible parameters will produce a null result + */ + @Deprecated + public static VelocityVector fromArcAndDestAndVel( + double speed, + double centerArcX, + double centerArcY, + double centerArcZ, + double arcRadius, + double destX, + double destY, + double destZ, + double degreeThreshold, + int maxIterations, + double maxFractionalRange) { + double xDist = destX - centerArcX; + double yDist = destY - centerArcY; + VelocityVector lastVel = null; + VelocityVector currVel = null; + int i = 0; + do { + i++; + double theta = Math.PI / 4; + if (currVel != null) { + lastVel = currVel; + theta = currVel.getAngle().getRadians(); + } + double rampXYOffset = (Math.cos(theta) * arcRadius); + double rampZOffset = Math.sin(theta) * arcRadius; + double xyDist = Math.hypot(xDist, yDist) - rampXYOffset; + double z1 = centerArcZ + rampZOffset; + double deltaZ = Math.abs(destZ - z1); + currVel = fromVelAndDist(speed, xyDist, deltaZ, maxFractionalRange); + } while (i < maxIterations + && (lastVel == null + || currVel == null + || Math.abs(lastVel.getAngle().getDegrees() - currVel.getAngle().getDegrees()) + > degreeThreshold)); + return currVel; + } + + /** + * The projectile motion converts the given coords to 2d after applying the change in base coords + * from the arc provided. Because shooter angle depends on position (not point math) and position + * depends on angle (not point math), the function will approximate to a specified certainty by + * calling the function at 45 degrees, getting that pose and then looping until the delta angle + * result is within 0.01 degrees, or you have exceeded 7 iterations.
+ * To use custom certainty see {@link #fromArcAndDestAndVel(double, double, double, double, + * double, double, double, double, double, int, double)} + * + * @param speed the initial velocity of the projectile + * @param centerArcX the x cord of the arc created by the shooter moving at different angles + * @param centerArcY the y cord of the arc created by the shooter moving at different angles + * @param centerArcZ the z cord of the arc created by the shooter moving at different angles + * @param arcRadius the length of the shooter, which forms an arc when changing angle + * @param destX the target position in the x direction + * @param destY the target position in the y direction + * @param destZ the target position in the z direction + * @return a {@link VelocityVector} with the calculated target angle between 0 and 90 degrees.
+ * Impossible parameters will produce a null result + */ + @Deprecated + public static VelocityVector fromArcAndDestAndVel( + double speed, + double centerArcX, + double centerArcY, + double centerArcZ, + double arcRadius, + double destX, + double destY, + double destZ) { + return fromArcAndDestAndVel( + speed, + centerArcX, + centerArcY, + centerArcZ, + arcRadius, + destX, + destY, + destZ, + 0.01, + 1, + 1.0 / 2); + } + + /** + * @param speed speed the projectile is moving at
+ * Must be > 0 + * @param deltaX the X distance between the starting and ending positions
+ * Must be > 0 + * @param deltaY the Y distance between the starting and ending positions
+ * Must be > 0 + * @param maxFractionalRange double representing what fraction of range of the parabola is + * acceptable for hitting your target.
+ * For example if you have to hit your target from bottom, you can constrain the parabola to + * only be acceptable in the first third of the parabola. + * @return a {@link VelocityVector} with the calculated target angle between 0 and 90 degrees.
+ * Impossible parameters will produce a null result + */ + public static VelocityVector fromVelAndDist( + double speed, double deltaX, double deltaY, double maxFractionalRange) { + if (speed <= 0 || deltaX <= 0 || deltaY <= 0) return null; + double tanOfAngle = + (((deltaY >= 0) ? 1 : -1) + * Math.sqrt( + Math.pow(deltaX, 2) + * ((-1 * Math.pow(Constants.GRAVITY, 2) * Math.pow(deltaX, 2)) + + (2 * Constants.GRAVITY * Math.pow(speed, 2) * deltaY) + + (Math.pow(speed, 4)))) + - (Math.pow(speed, 2) * deltaX)) + / (Constants.GRAVITY * Math.pow(deltaX, 2)); + VelocityVector velocityVector = + new VelocityVector(speed, new Rotation2d(Math.atan(tanOfAngle))); + return horizontalRange(velocityVector) * maxFractionalRange > deltaX ? velocityVector : null; + } + + /** + * @param velocityVector the initial velocity and angle of the projectile + * @return the max range of a projectile following a theoretical parabola + */ + public static double horizontalRange(VelocityVector velocityVector) { + return ((Math.pow(velocityVector.getVelocity(), 2) + * Math.sin(velocityVector.getAngle().getRadians() * 2))) + / (-1 * Constants.GRAVITY); + } + + /** + * @param velocityVector the initial velocity and angle of the projectile + * @return the max height of a projectile following a theoretical parabola + */ + public static double maximumHeight(VelocityVector velocityVector) { + return (Math.pow(velocityVector.getVelocity(), 2) + * Math.pow(Math.sin(velocityVector.getAngle().getRadians()), 2)) + / (2 * Constants.GRAVITY * -1); + } + + /** + * The projectile motion converts the given coords to 2d and after applying the drivetrain x + * velocity, to the x component of the projectile's initial velocity.
+ * Because shooter component velocities depends on the angle we shoot at, and the angle we shoot + * depends on the composition of the component velocities. if we want to incorporate the + * drivetrains velocity we must use an iterative approach.
+ * This function will approximate to a specified certainty by assuming a shot of 45 degrees and + * splitting the velocity into its components. Then the function adds the drivetrains x velocity + * to the x component and returns a new shooting angle based the combined vectors.
+ * This process repeats until max iterations is reached (returns the current angle) or the degree + * threshold between iterations is less than the threshold provided + * + * @param speed the initial velocity of the projectile + * @param startX the x cord of the base of the ramp + * @param startY the y cord of the base of the ramp + * @param startZ the z cord of the base of the ramp + * @param driveSpeedX the drivetrain velocity in meters per second + * @param destX the destination x position + * @param destY the destination y position + * @param destZ the destination z position + * @param degreeThreshold the degree threshold between iteration before function is considered + * done + * @param maxIterations the max iteration before iteration is considered done. Note it does not + * require that max iterations was reached if degree threshold predicate is met. + * @param maxFractionalRange double representing what fraction of range of the parabola is + * acceptable for hitting your target.
+ * For example if you have to hit your target from bottom, you can constrain the parabola to + * only be acceptable in the first third of the parabola. + * @return a {@link VelocityVector} with the calculated target angle between 0 and 90 degrees.
+ * Impossible parameters will produce a null result + */ + // public static VelocityVector fromDestAndCompoundVel(double speed, double startX, double startY, + // double startZ, double driveSpeedX, double destX, double destY, double destZ,double distOffset + // ,double degreeThreshold, int maxIterations, double maxFractionalRange) { + // double xDist = destX - startX; + // double yDist = destY - startY; + // double xyDist = Math.hypot(xDist, yDist) + distOffset; + // double deltaZ = Math.abs(destZ - startZ); + // VelocityVector lastVel = null; + // VelocityVector currVel = null; + // int i = 0; + // do { + // i++; + // lastVel = currVel; + // double theta = lastVel == null ? Math.PI / 4 : lastVel.getAngle().getRadians(); + // double xySpeed = (Math.cos(theta) * speed) + driveSpeedX; + // double zSpeed = Math.sin(theta) * speed; + // double appliedSpeed = Math.hypot(xySpeed, zSpeed); + // currVel = fromVelAndDist(appliedSpeed, xyDist, deltaZ, maxFractionalRange); + // } while (i < maxIterations && (lastVel == null || currVel == null || + // Math.abs(lastVel.getAngle().getDegrees() - currVel.getAngle().getDegrees()) > + // degreeThreshold)); + // return currVel == null ? new VelocityVector(Constants.SHOOTER_VELOCITY, new Rotation2d()) : + // currVel; + // } + + /** + * The projectile motion converts the given coords to 2d and after applying the drivetrain x + * velocity, to the x component of the projectile's initial velocity.
+ * Because shooter component velocities depends on the angle we shoot at, and the angle we shoot + * depends on the composition of the component velocities. if we want to incorporate the + * drivetrains velocity we must use an iterative approach.
+ * This function will approximate to a specified certainty by assuming a shot of 45 degrees and + * splitting the velocity into its components. Then the function adds the drivetrains x velocity + * to the x component and returns a new shooting angle based the combined vectors.
+ * This process repeats until the 10 iterations is reached (returns the current angle) or the + * degree threshold of 0.01 between iterations is less than the threshold provided + * + * @param speed the initial velocity of the projectile + * @param startX the x cord of the base of the ramp + * @param startY the y cord of the base of the ramp + * @param startZ the z cord of the base of the ramp + * @param driveSpeedX the drivetrain velocity in meters per second + * @param destX the destination x position + * @param destY the destination y position + * @param destZ the destination z position + * @return a {@link VelocityVector} with the calculated target angle between 0 and 90 degrees.
+ * Impossible parameters will produce a null result + */ + // public static VelocityVector fromDestAndCompoundVel(double speed, double startX, double startY, + // double startZ, double driveSpeedX, double destX, double destY, double destZ) { + // return fromDestAndCompoundVel(speed, startX, startY, startZ, driveSpeedX, destX, destY, + // destZ, Constants.RAMP_FROM_CENTER,0.01, 10, 2.0/5); + // } + + public static boolean isErrorSafe( + VelocityVector velocityVector, + Rotation2d angleError, + double destX, + double destZ, + double maxErrorZ) { + double highY = + getYAtX( + new VelocityVector( + velocityVector.getVelocity(), velocityVector.getAngle().plus(angleError)), + destX); + double lowY = + getYAtX( + new VelocityVector( + velocityVector.getVelocity(), velocityVector.getAngle().minus(angleError)), + destX); + Logger.recordOutput("VectorUtils/maxDiffY", highY - destZ); + Logger.recordOutput("VectorUtils/minDiffY", destZ - lowY); + return highY < destZ + maxErrorZ && lowY > destZ - maxErrorZ; + } + + public static double getYAtX(VelocityVector velocityVector, double x) { + double time = getTimeForXDist(velocityVector, x); + return velocityVector.getYSpeed() * time + (Constants.GRAVITY * 0.5 * Math.pow(time, 2)); + } + + public static double getTimeForXDist(VelocityVector velocityVector, double x) { + return x / velocityVector.getXSpeed(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/math/VelocityVector.java b/src/main/java/frc/robot/utils/math/VelocityVector.java new file mode 100644 index 0000000..29bd5c3 --- /dev/null +++ b/src/main/java/frc/robot/utils/math/VelocityVector.java @@ -0,0 +1,29 @@ +package frc.robot.utils.math; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class VelocityVector { + private final Rotation2d angle; + private final double velocity; + + public VelocityVector(double speed, Rotation2d angle) { + this.velocity = speed; + this.angle = angle; + } + + public double getXSpeed() { + return Math.cos(angle.getRadians()) * velocity; + } + + public double getYSpeed() { + return Math.sin(angle.getRadians()) * velocity; + } + + public double getVelocity() { + return velocity; + } + + public Rotation2d getAngle() { + return angle; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/motor/Gain.java b/src/main/java/frc/robot/utils/motor/Gain.java new file mode 100644 index 0000000..4ac9253 --- /dev/null +++ b/src/main/java/frc/robot/utils/motor/Gain.java @@ -0,0 +1,23 @@ +package frc.robot.utils.motor; + +public class Gain { + private final double v; + private final double s; + + public Gain(double v, double s) { + this.v = v; + this.s = s; + } + + public double getV() { + return v; + } + + public double getS() { + return s; + } + + public static Gain of(double v, double s) { + return new Gain(v, s); + } +} diff --git a/src/main/java/frc/robot/utils/motor/NeoPidConfig.java b/src/main/java/frc/robot/utils/motor/NeoPidConfig.java new file mode 100644 index 0000000..369488b --- /dev/null +++ b/src/main/java/frc/robot/utils/motor/NeoPidConfig.java @@ -0,0 +1,127 @@ +package frc.robot.utils.motor; + +public class NeoPidConfig { + public static final double DEFAULT_P = 0.01; + public static final double DEFAULT_I = 0; + public static final double DEFAULT_D = 0.0; + public static final double DEFAULT_IZONE = 0.0; + public static final double DEFAULT_FF = 0.0; + public static final double MAX_VELOCITY = 5000; + public static final double MAX_ACCELERATION = 10000; + public static final double ALLOWED_ERROR = 1.0; + + private double p = DEFAULT_P; + private double i = DEFAULT_I; + private double d = DEFAULT_D; + private double iZone = DEFAULT_IZONE; + private double ff = DEFAULT_FF; + private int currentLimit; // TODO: SHould this be set to 40 as a default? + private double maxVelocity = MAX_VELOCITY; + private double maxAccel = MAX_ACCELERATION; + private double allowedError = ALLOWED_ERROR; + private boolean usesMaxMotion; + + public NeoPidConfig(boolean usesMaxMotion) { + this.usesMaxMotion = usesMaxMotion; + } + + public double getP() { + return p; + } + + public NeoPidConfig setP(double p) { + this.p = p; + return this; + } + + public double getI() { + return i; + } + + public NeoPidConfig setI(double i) { + this.i = i; + return this; + } + + public double getD() { + return d; + } + + public NeoPidConfig setD(double d) { + this.d = d; + return this; + } + + public double getIZone() { + return iZone; + } + + public NeoPidConfig setIZone(double iZone) { + this.iZone = iZone; + return this; + } + + public double getFF() { + return ff; + } + + public NeoPidConfig setFF(double ff) { + this.ff = ff; + return this; + } + + public int getCurrentLimit() { + return currentLimit; + } + + public NeoPidConfig setCurrentLimit(int currentLimit) { + this.currentLimit = currentLimit; + return this; + } + + public NeoPidConfig setMaxVelocity(double maxVelocity) { + this.maxVelocity = maxVelocity; + return this; + } + + public NeoPidConfig setMaxAccel(double maxAccel) { + this.maxAccel = maxAccel; + return this; + } + + public NeoPidConfig setAllowedError(double allowedError) { + this.allowedError = allowedError; + return this; + } + + public NeoPidConfig setPid(double p, double i, double d) { + setP(p).setI(i).setD(d); + return this; + } + + public NeoPidConfig setPidf(double p, double i, double d, double ff) { + setPid(p, i, d).setFF(ff); + return this; + } + + public NeoPidConfig setTrapezoidConstructions(double maxVelocity, double maxAccel) { + setMaxVelocity(maxVelocity).setMaxAccel(maxAccel); + return this; + } + + public double getMaxVelocity() { + return maxVelocity; + } + + public double getMaxAccel() { + return maxAccel; + } + + public double getAllowedError() { + return allowedError; + } + + public boolean getUsesMaxMotion() { + return usesMaxMotion; + } +} diff --git a/src/main/java/frc/robot/utils/motor/NeoPidMotor.java b/src/main/java/frc/robot/utils/motor/NeoPidMotor.java new file mode 100644 index 0000000..92db319 --- /dev/null +++ b/src/main/java/frc/robot/utils/motor/NeoPidMotor.java @@ -0,0 +1,162 @@ +package frc.robot.utils.motor; + +import static com.revrobotics.spark.SparkBase.PersistMode.kNoPersistParameters; +import static com.revrobotics.spark.SparkBase.PersistMode.kPersistParameters; +import static com.revrobotics.spark.SparkBase.ResetMode.kNoResetSafeParameters; +import static com.revrobotics.spark.SparkBase.ResetMode.kResetSafeParameters; +import static com.revrobotics.spark.config.SparkBaseConfig.IdleMode.kBrake; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.*; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.LimitSwitchConfig; +import com.revrobotics.spark.config.MAXMotionConfig.MAXMotionPositionMode; +import com.revrobotics.spark.FeedbackSensor; + +import frc.robot.utils.logging.io.motor.SparkMaxIo; + +import com.revrobotics.spark.config.SparkMaxConfig; + +/** + * A Wrapper utility to encapsulate the NEO motor with PID capability. This is simply a wrapper with + * some convenient defaults and initializations that make programming the PID of the NEO easier. + * + *

TODO: This does not yet support the external absolute encoder that may be needed TODO: This + * does not yet support velocity PID or other advanced features + */ +public class NeoPidMotor { + + public static final double RAMP_RATE = 0; + + // The neo motor controller + private final SparkMax neoMotor; + private final NeoPidConfig pidConfig; // if using arbff we need to keep track of pidConfig + // The built-in relative encoder + private final RelativeEncoder encoder; + // The built-in PID controller + private final SparkClosedLoopController pidController; + + // The desired motor position + private double setPosition = 0.0; + + /** + * Constructor using reasonable default values + * + * @param id the CAN ID for the controller + */ + public NeoPidMotor(int id, boolean usesMaxMotion) { + this(id, new NeoPidConfig(usesMaxMotion)); + } + + public NeoPidMotor(int id, NeoPidConfig pidConfig) { + neoMotor = new SparkMax(id, SparkLowLevel.MotorType.kBrushless); + this.pidConfig = pidConfig; + encoder = neoMotor.getEncoder(); + + pidController = neoMotor.getClosedLoopController(); + SparkMaxConfig config = new SparkMaxConfig(); + config + .smartCurrentLimit(pidConfig.getCurrentLimit()) + .closedLoopRampRate(RAMP_RATE) + .idleMode(kBrake); + config + .closedLoop + .feedbackSensor(FeedbackSensor.kPrimaryEncoder) + .pid(pidConfig.getP(), pidConfig.getI(), pidConfig.getD()) + .velocityFF(pidConfig.getFF()) + .iZone(pidConfig.getIZone()) + .outputRange(-1, 1); + if (pidConfig.getUsesMaxMotion()) { + config + .closedLoop + .maxMotion + .maxVelocity(pidConfig.getMaxVelocity()) + .maxAcceleration(pidConfig.getMaxAccel()) + .positionMode(MAXMotionPositionMode.kMAXMotionTrapezoidal) + .allowedClosedLoopError(pidConfig.getAllowedError()); + } + + config + .limitSwitch + .forwardLimitSwitchEnabled(true) + .forwardLimitSwitchType(LimitSwitchConfig.Type.kNormallyOpen) + .reverseLimitSwitchEnabled(true) + .forwardLimitSwitchType(LimitSwitchConfig.Type.kNormallyOpen); + + neoMotor.configure(config, kResetSafeParameters, kPersistParameters); + } + + /** + * Reconfigure the PID fully using all values from motor params + * + * @param params + */ + public void configurePID(NeoPidConfig params) { + SparkMaxConfig config = new SparkMaxConfig(); + config + .closedLoop + .pid(params.getP(), params.getI(), params.getD()) + .velocityFF(params.getFF()) + .iZone(params.getIZone()); + if (params.getUsesMaxMotion()) { + config + .closedLoop + .maxMotion + .maxVelocity(params.getMaxVelocity()) + .maxAcceleration(params.getMaxAccel()) + .allowedClosedLoopError(params.getAllowedError()); + } + neoMotor.configure(config, kNoResetSafeParameters, kNoPersistParameters); + } + + /** + * Set the desired position using the relative encoder as a reference + * + * @param position the desired motor position + */ + public void setPidPos(double position, SparkBase.ControlType controlType) { + pidController.setReference(position, controlType); + this.setPosition = position; + } + + public double getPidPosition() { + return setPosition; + } + + public void setPid(double pidP, double pidI, double pidD) { + SparkMaxConfig config = new SparkMaxConfig(); + config.closedLoop.pid(pidP, pidI, pidD); + neoMotor.configure(config, kNoResetSafeParameters, kNoPersistParameters); + } + + public void setPid(double pidP, double pidI, double pidD, double iZone, double pidFF) { + SparkMaxConfig config = new SparkMaxConfig(); + config.closedLoop.pid(pidP, pidI, pidD).velocityFF(pidFF).iZone(iZone); + neoMotor.configure(config, kNoResetSafeParameters, kNoPersistParameters); + } + + public SparkMax getNeoMotor() { + return neoMotor; + } + + public RelativeEncoder getEncoder() { + return encoder; + } + + public SparkClosedLoopController getPidController() { + return pidController; + } + + public void configure(NeoPidConfig config) { + SparkMaxConfig sparkMaxConfig = new SparkMaxConfig(); + sparkMaxConfig + .closedLoop + .pidf(config.getP(), config.getI(), config.getD(), config.getFF()) + .iZone(config.getIZone()) + .maxMotion + .maxVelocity(config.getMaxVelocity()) + .maxAcceleration(config.getMaxAccel()) + .allowedClosedLoopError(config.getAllowedError()); + getNeoMotor().configure(sparkMaxConfig, kResetSafeParameters, kNoPersistParameters); + } +} diff --git a/src/main/java/frc/robot/utils/motor/PID.java b/src/main/java/frc/robot/utils/motor/PID.java new file mode 100644 index 0000000..6bcea0f --- /dev/null +++ b/src/main/java/frc/robot/utils/motor/PID.java @@ -0,0 +1,29 @@ +package frc.robot.utils.motor; + +public class PID { + private final double p; + private final double i; + private final double d; + + public PID(double p, double i, double d) { + this.p = p; + this.i = i; + this.d = d; + } + + public double getP() { + return p; + } + + public double getI() { + return i; + } + + public double getD() { + return d; + } + + public static PID of(double p, double i, double d) { + return new PID(p, i, d); + } +} diff --git a/src/main/java/frc/robot/utils/motor/TunablePIDManager.java b/src/main/java/frc/robot/utils/motor/TunablePIDManager.java new file mode 100644 index 0000000..10327ed --- /dev/null +++ b/src/main/java/frc/robot/utils/motor/TunablePIDManager.java @@ -0,0 +1,57 @@ +package frc.robot.utils.motor; + +import frc.robot.utils.logging.LoggedTunableNumber; +import frc.robot.utils.logging.PIDIo; + +public class TunablePIDManager { + private final PIDIo io; + private final NeoPidConfig initConfig; + private final String prefix; + private final LoggedTunableNumber kPTunable; + private final LoggedTunableNumber kITunable; + private final LoggedTunableNumber kDTunable; + private final LoggedTunableNumber kIZoneTunable; + private final LoggedTunableNumber kFFTunable; + private final LoggedTunableNumber kMaxVelTunable; + private final LoggedTunableNumber kMaxAccTunable; + private final LoggedTunableNumber kAllowedErrorTunable; + + public TunablePIDManager(String prefix, PIDIo io, NeoPidConfig initConfig) { + this.io = io; + this.initConfig = initConfig; + this.prefix = prefix; + kPTunable = new LoggedTunableNumber(prefix + "/PID_P", initConfig.getP()); + kITunable = new LoggedTunableNumber(prefix + "/PID_I", initConfig.getI()); + kDTunable = new LoggedTunableNumber(prefix + "/PID_D", initConfig.getD()); + kIZoneTunable = new LoggedTunableNumber(prefix + "/PID_I_ZONE", initConfig.getIZone()); + kFFTunable = new LoggedTunableNumber(prefix + "/PID_FF", initConfig.getFF()); + kMaxVelTunable = new LoggedTunableNumber(prefix + "/PID_MAX_VEL", initConfig.getMaxVelocity()); + kMaxAccTunable = new LoggedTunableNumber(prefix + "/PID_MAX_ACCEL", initConfig.getMaxAccel()); + kAllowedErrorTunable = + new LoggedTunableNumber(prefix + "/PID_ALLOWED_ERROR", initConfig.getAllowedError()); + } + + public void periodic() { + LoggedTunableNumber.ifChanged( + hashCode(), + () -> + io.configurePID( + new NeoPidConfig(initConfig.getUsesMaxMotion()) + .setP(kPTunable.get()) + .setI(kITunable.get()) + .setD(kDTunable.get()) + .setIZone(kIZoneTunable.get()) + .setFF(kFFTunable.get()) + .setMaxVelocity(kMaxVelTunable.get()) + .setMaxAccel(kMaxAccTunable.get()) + .setAllowedError(kAllowedErrorTunable.get())), + kPTunable, + kITunable, + kDTunable, + kIZoneTunable, + kFFTunable, + kMaxVelTunable, + kMaxAccTunable, + kAllowedErrorTunable); + } +}