diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 784e1ba..d26bd41 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,7 +1,7 @@ package frc.robot; import edu.wpi.first.wpilibj.RobotBase; -import frc.robot.utility.Alert; +import frc.robot.utility.logging.Alert; /** * The Constants class provides a convenient place to hold robot-wide numerical or boolean diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 85fe84d..d9eb6ea 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,11 +36,11 @@ import frc.robot.subsystems.vision.AprilTagVision; import frc.robot.subsystems.vision.CameraIOSim; import frc.robot.subsystems.vision.VisionConstants; -import frc.robot.utility.Alert; -import frc.robot.utility.Alert.AlertType; import frc.robot.utility.OverrideSwitch; import frc.robot.utility.SpeedController; import frc.robot.utility.SpeedController.SpeedLevel; +import frc.robot.utility.logging.Alert; +import frc.robot.utility.logging.Alert.AlertType; import java.util.Optional; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -69,7 +69,7 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { switch (Constants.getRobot()) { - case COMP_BOT: + case COMP_BOT, DEV_BOT: // Real robot, instantiate hardware IO implementations drive = new Drive( @@ -82,19 +82,6 @@ public RobotContainer() { vision = new AprilTagVision(); break; - case DEV_BOT: - // Real robot, instantiate hardware IO implementations - drive = - new Drive( - new GyroIOPigeon2(false), - new ModuleIOSparkMax(DriveConstants.FRONT_LEFT_MODULE_CONFIG), - new ModuleIOSparkMax(DriveConstants.FRONT_RIGHT_MODULE_CONFIG), - new ModuleIOSparkMax(DriveConstants.BACK_LEFT_MODULE_CONFIG), - new ModuleIOSparkMax(DriveConstants.BACK_RIGHT_MODULE_CONFIG)); - flywheelExample = new Flywheel(new FlywheelIOSparkMax()); - vision = new AprilTagVision(); - break; - case OLD_DEV_BOT: // Real robot, instantiate hardware IO implementations drive = @@ -207,12 +194,18 @@ private void configureDriverControllerBindings() { final Trigger useFieldRelative = new Trigger( new OverrideSwitch( - driverXbox.y(), "Field Relative", OverrideSwitch.Mode.TOGGLE, true)); + driverXbox.y(), + OverrideSwitch.Mode.TOGGLE, + true, + (state) -> SmartDashboard.putBoolean("Field Relative", state))); final Trigger useAngleControlMode = new Trigger( new OverrideSwitch( - driverXbox.rightBumper(), "Angle Driven", OverrideSwitch.Mode.HOLD, false)); + driverXbox.rightBumper(), + OverrideSwitch.Mode.HOLD, + false, + (state) -> SmartDashboard.putBoolean("Angle Driven", state))); // Controllers final TeleopDriveController input = @@ -452,9 +445,10 @@ public void initSmartDashboardOutputs() { } public void updateSmartDashboardOutputs() { - Pose2d pose = drive.getPose(); + Pose2d pose = drive.getPose(); ChassisSpeeds speeds = drive.getRobotSpeeds(); + SmartDashboard.putNumber("Heading Degrees", -pose.getRotation().getDegrees()); SmartDashboard.putNumber( "Speed MPH", Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond) * 2.2369); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index cf19b62..59594a7 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -4,8 +4,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import frc.robot.Constants; -import frc.robot.utility.FeedForwardConstants; -import frc.robot.utility.PIDConstants; +import frc.robot.utility.records.FeedForwardConstants; +import frc.robot.utility.records.PIDConstants; import frc.robot.utility.swerve254util.ModuleLimits; /** diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index a0b75e7..8f68b24 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -8,8 +8,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.robot.utility.LoggedTunableNumber; -import frc.robot.utility.LoggedTunableNumberGroup; +import frc.robot.utility.logging.LoggedTunableNumber; +import frc.robot.utility.logging.LoggedTunableNumberGroup; import org.littletonrobotics.junction.Logger; /** diff --git a/src/main/java/frc/robot/subsystems/drive/controllers/HeadingController.java b/src/main/java/frc/robot/subsystems/drive/controllers/HeadingController.java index 28a8d16..97aa579 100644 --- a/src/main/java/frc/robot/subsystems/drive/controllers/HeadingController.java +++ b/src/main/java/frc/robot/subsystems/drive/controllers/HeadingController.java @@ -10,8 +10,8 @@ import frc.robot.RobotState; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; -import frc.robot.utility.LoggedTunableNumber; -import frc.robot.utility.LoggedTunableNumberGroup; +import frc.robot.utility.logging.LoggedTunableNumber; +import frc.robot.utility.logging.LoggedTunableNumberGroup; import frc.robot.utility.swerve254util.ModuleLimits; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/subsystems/drive/controllers/TeleopDriveController.java b/src/main/java/frc/robot/subsystems/drive/controllers/TeleopDriveController.java index 8dbb76a..48a9ce9 100644 --- a/src/main/java/frc/robot/subsystems/drive/controllers/TeleopDriveController.java +++ b/src/main/java/frc/robot/subsystems/drive/controllers/TeleopDriveController.java @@ -7,8 +7,8 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.subsystems.drive.Drive; -import frc.robot.utility.LoggedTunableNumber; -import frc.robot.utility.LoggedTunableNumberGroup; +import frc.robot.utility.logging.LoggedTunableNumber; +import frc.robot.utility.logging.LoggedTunableNumberGroup; import java.util.Optional; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/robot/subsystems/vision/Camera.java b/src/main/java/frc/robot/subsystems/vision/Camera.java index 1e5b833..c3c9076 100644 --- a/src/main/java/frc/robot/subsystems/vision/Camera.java +++ b/src/main/java/frc/robot/subsystems/vision/Camera.java @@ -9,9 +9,9 @@ import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; -import frc.robot.utility.Alert; -import frc.robot.utility.LoggedTunableNumber; -import frc.robot.utility.LoggedTunableNumberGroup; +import frc.robot.utility.logging.Alert; +import frc.robot.utility.logging.LoggedTunableNumber; +import frc.robot.utility.logging.LoggedTunableNumberGroup; import java.util.Arrays; import java.util.DoubleSummaryStatistics; import java.util.Optional; diff --git a/src/main/java/frc/robot/utility/OverrideSwitch.java b/src/main/java/frc/robot/utility/OverrideSwitch.java index c00f830..f7789b4 100644 --- a/src/main/java/frc/robot/utility/OverrideSwitch.java +++ b/src/main/java/frc/robot/utility/OverrideSwitch.java @@ -1,16 +1,16 @@ package frc.robot.utility; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; import java.util.function.BooleanSupplier; +import java.util.function.Consumer; /** Class for switching on and off features, implements BooleanSupplier */ public class OverrideSwitch implements BooleanSupplier { private boolean value; - private final String name; + private final Consumer onUpdate; public static enum Mode { TOGGLE, @@ -21,13 +21,12 @@ public static enum Mode { * Creates new Override switch * * @param trigger trigger to toggle state - * @param name name for smart dashboard * @param mode either toggle or hold * @param defaultState default state + * @param onUpdate runnable which takes in new state */ - public OverrideSwitch(Trigger trigger, String name, Mode mode, boolean defaultState) { - this.name = name; - + public OverrideSwitch( + Trigger trigger, Mode mode, boolean defaultState, Consumer onUpdate) { switch (mode) { case TOGGLE: trigger.onTrue(Commands.runOnce(this::toggle)); @@ -41,15 +40,14 @@ public OverrideSwitch(Trigger trigger, String name, Mode mode, boolean defaultSt break; } + this.onUpdate = onUpdate; + set(defaultState); } private void set(boolean value) { this.value = value; - - if (name != null) { - SmartDashboard.putBoolean(name, value); - } + onUpdate.accept(this.value); } public void toggle() { diff --git a/src/main/java/frc/robot/utility/Alert.java b/src/main/java/frc/robot/utility/logging/Alert.java similarity index 91% rename from src/main/java/frc/robot/utility/Alert.java rename to src/main/java/frc/robot/utility/logging/Alert.java index d3a8b1e..10449d6 100644 --- a/src/main/java/frc/robot/utility/Alert.java +++ b/src/main/java/frc/robot/utility/logging/Alert.java @@ -1,4 +1,4 @@ -package frc.robot.utility; +package frc.robot.utility.logging; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; @@ -6,11 +6,9 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.ArrayList; -import java.util.Comparator; import java.util.HashMap; import java.util.List; import java.util.Map; -import java.util.function.Predicate; /** Class for managing persistent alerts to be sent over NetworkTables. */ public class Alert { @@ -92,13 +90,10 @@ private static class SendableAlerts implements Sendable { public final List alerts = new ArrayList<>(); public String[] getStrings(AlertType type) { - Predicate activeFilter = (Alert x) -> x.type == type && x.active; - Comparator timeSorter = - (Alert a1, Alert a2) -> (int) (a2.activeStartTime - a1.activeStartTime); return alerts.stream() - .filter(activeFilter) - .sorted(timeSorter) - .map((Alert a) -> a.text) + .filter(a -> a.type == type && a.active) + .sorted((a1, a2) -> Double.compare(a2.activeStartTime, a1.activeStartTime)) + .map(a -> a.text) .toArray(String[]::new); } diff --git a/src/main/java/frc/robot/utility/LoggedTunableNumber.java b/src/main/java/frc/robot/utility/logging/LoggedTunableNumber.java similarity index 98% rename from src/main/java/frc/robot/utility/LoggedTunableNumber.java rename to src/main/java/frc/robot/utility/logging/LoggedTunableNumber.java index 2cdcf04..fe69594 100644 --- a/src/main/java/frc/robot/utility/LoggedTunableNumber.java +++ b/src/main/java/frc/robot/utility/logging/LoggedTunableNumber.java @@ -1,4 +1,4 @@ -package frc.robot.utility; +package frc.robot.utility.logging; import frc.robot.Constants; import java.util.Arrays; diff --git a/src/main/java/frc/robot/utility/LoggedTunableNumberGroup.java b/src/main/java/frc/robot/utility/logging/LoggedTunableNumberGroup.java similarity index 92% rename from src/main/java/frc/robot/utility/LoggedTunableNumberGroup.java rename to src/main/java/frc/robot/utility/logging/LoggedTunableNumberGroup.java index 9b0a73a..07655dc 100644 --- a/src/main/java/frc/robot/utility/LoggedTunableNumberGroup.java +++ b/src/main/java/frc/robot/utility/logging/LoggedTunableNumberGroup.java @@ -1,4 +1,4 @@ -package frc.robot.utility; +package frc.robot.utility.logging; public class LoggedTunableNumberGroup { private final String root; diff --git a/src/main/java/frc/robot/utility/FeedForwardConstants.java b/src/main/java/frc/robot/utility/records/FeedForwardConstants.java similarity index 85% rename from src/main/java/frc/robot/utility/FeedForwardConstants.java rename to src/main/java/frc/robot/utility/records/FeedForwardConstants.java index b8260dd..2cb6268 100644 --- a/src/main/java/frc/robot/utility/FeedForwardConstants.java +++ b/src/main/java/frc/robot/utility/records/FeedForwardConstants.java @@ -1,4 +1,4 @@ -package frc.robot.utility; +package frc.robot.utility.records; /** Store gains for simple motor feed forward controller */ public record FeedForwardConstants(double kS, double kV, double kA) { diff --git a/src/main/java/frc/robot/utility/PIDConstants.java b/src/main/java/frc/robot/utility/records/PIDConstants.java similarity index 56% rename from src/main/java/frc/robot/utility/PIDConstants.java rename to src/main/java/frc/robot/utility/records/PIDConstants.java index 2d2dcd6..c1745cc 100644 --- a/src/main/java/frc/robot/utility/PIDConstants.java +++ b/src/main/java/frc/robot/utility/records/PIDConstants.java @@ -1,11 +1,14 @@ -package frc.robot.utility; +package frc.robot.utility.records; -/** Store gains in feed forward controller */ +/** Store gains in PID controller */ public record PIDConstants(double kP, double kI, double kD) { + + /** Proportional Controller */ public PIDConstants(double kP) { this(kP, 0, 0); } + /** Proportional Derivative Controller */ public PIDConstants(double kP, double kD) { this(kP, 0, kD); }