Dashboard changes

src/main/java/frc/robot/Autos.java

@@ -2,25 +2,63 @@
 
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue;
+
+import java.util.stream.IntStream;
+
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
 import choreo.auto.AutoFactory;
 import choreo.trajectory.SwerveSample;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.Drive;
 
 public class Autos
 {
+    private enum StartPosition
+    {
+        LeftTrench("Left Trench"), LeftBump("Left Bump"), Hub("Hub"), RightBump("Right Bump"), RightTrench("Right Trench");
+
+        private final String displayName;
+
+        private StartPosition(String displayName)
+        {
+            this.displayName = displayName;
+        }
+    }
+
+    private enum AllianceFuelSourceSelection
+    {
+        None("None"), Depot("Depot"), Outpost("Outpost"), DepotToOutpost("Depot to Outpost"), OutpostToDepot("Outpost to Depot");
+
+        private final String displayName;
+
+        private AllianceFuelSourceSelection(String displayName)
+        {
+            this.displayName = displayName;
+        }
+    }
+
     private final AutoFactory                _autoFactory;
     private final Drive                      _driveSubsystem;
     private final SwerveRequest.FieldCentric _autoFollowingRequest = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);
     private final PIDController              _xController          = new PIDController(0.0, 0.0, 0.0);
     private final PIDController              _yController          = new PIDController(0.0, 0.0, 0.0);
     private final PIDController              _headingController    = new PIDController(0.0, 0.0, 0.0);
 
+    // Dashboard
+    private final Field2d                                      _field;
+    private final SendableChooser<Integer>                     _autoDelayChooser;
+    private final SendableChooser<StartPosition>               _startPositionChooser;
+    private final SendableChooser<AllianceFuelSourceSelection> _allianceFuelSourceSelectionChooser;
+    private final String                                       _neutralZoneNTKey;
+    private final String                                       _climbNTKey;
+
     public Autos(Drive driveSubsystem)
     {
         _driveSubsystem = driveSubsystem;
@@ -35,6 +73,45 @@ public Autos(Drive driveSubsystem)
             driveSubsystem
         );
         // @formatter:on
+
+        // Dashboard
+        _field = new Field2d();
+
+        _autoDelayChooser                   = new SendableChooser<>();
+        _startPositionChooser               = new SendableChooser<>();
+        _allianceFuelSourceSelectionChooser = new SendableChooser<>();
+
+        _neutralZoneNTKey = "Neutral Zone";
+        _climbNTKey       = "Climb";
+
+        _autoDelayChooser.setDefaultOption("0", 0);
+        IntStream.range(1, 6).forEach(n -> _autoDelayChooser.addOption(String.valueOf(n), n));
+
+        var startPositions = StartPosition.values();
+        var fuelSources    = AllianceFuelSourceSelection.values();
+
+        _startPositionChooser.setDefaultOption(startPositions[0].displayName, startPositions[0]);
+        _allianceFuelSourceSelectionChooser.setDefaultOption(fuelSources[0].displayName, fuelSources[0]);
+
+        for (int i = 1; i < startPositions.length; i++)
+        {
+            _startPositionChooser.addOption(startPositions[i].displayName, startPositions[i]);
+        }
+
+        for (int i = 1; i < fuelSources.length; i++)
+        {
+            _allianceFuelSourceSelectionChooser.addOption(fuelSources[i].displayName, fuelSources[i]);
+        }
+
+        SmartDashboard.putData("Autonomous Mode", _field);
+        SmartDashboard.putData("Auto Delay", _autoDelayChooser);
+        SmartDashboard.putData("Start Position", _startPositionChooser);
+        SmartDashboard.putData("Alliance Fuel Source", _allianceFuelSourceSelectionChooser);
+        SmartDashboard.putData("Auto X PID", _xController);
+        SmartDashboard.putData("Auto Y PID", _yController);
+        SmartDashboard.putData("Auto Heading PID", _headingController);
+        SmartDashboard.putBoolean(_neutralZoneNTKey, false);
+        SmartDashboard.putBoolean(_climbNTKey, false);
     }
 
     private void followTrajectory(SwerveSample sample)

src/main/java/frc/robot/Robot.java

@@ -9,6 +9,8 @@
 
 import edu.wpi.first.epilogue.Epilogue;
 import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.net.WebServer;
+import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -28,6 +30,10 @@ public Robot()
     {
         m_robotContainer = new RobotContainer();
         Epilogue.bind(this);
+
+        // Start a webserver in the deploy directory so Elastic can remotely download
+        // layouts
+        WebServer.start(5800, Filesystem.getDeployDirectory().getPath());
     }
 
     @Override

src/main/java/frc/robot/RobotContainer.java

@@ -24,6 +24,7 @@
 import frc.robot.Constants.DriveConstants;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.Drive;
+import frc.robot.subsystems.dashboard.Dashboard;
 import frc.robot.subsystems.intake.Intake;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.Turret;
@@ -44,8 +45,9 @@ public class RobotContainer
     private final Drive                          _drivetrain = TunerConstants.createDrivetrain();
     private final Intake                         _intake     = new Intake();
     private final Shooter                        _shooter    = new Shooter();
-    // private final Turret _turret = new Turret(_drivetrain::getState);
-    private final Autos _autos = new Autos(_drivetrain);
+    private final Turret                         _turret     = new Turret(_drivetrain::getState);
+    private final Autos                          _autos      = new Autos(_drivetrain);
+    private final Dashboard                      _dashboard  = new Dashboard(_intake, _shooter, _turret);
 
     public RobotContainer()
     {

src/main/java/frc/robot/subsystems/dashboard/Dashboard.java

@@ -0,0 +1,142 @@
+package frc.robot.subsystems.dashboard;
+
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.RPM;
+import static edu.wpi.first.units.Units.RPM;
+import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond;
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.units.Units.Value;
+import static edu.wpi.first.units.Units.Volts;
+
+import edu.wpi.first.networktables.BooleanPublisher;
+import edu.wpi.first.networktables.DoublePublisher;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Preferences;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.ClimberConstants;
+import frc.robot.Constants.IntakeConstants;
+import frc.robot.Constants.ShooterConstants;
+import frc.robot.Constants.VisionConstants;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.shooter.Hood.HoodPosition;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.Turret;
+import frc.robot.util.Utilities;
+
+public class Dashboard extends SubsystemBase
+{
+    private final Intake           _intake;
+    private final Shooter          _shooter;
+    private final Turret           _turret;
+    private final DoublePublisher  _matchTimePublisher;
+    private final BooleanPublisher _hubActivePublisher;
+    private final DoublePublisher  _hubStateTimeRemainingPublisher;
+    private final BooleanPublisher _turretHasTargetPublisher;
+    private final BooleanPublisher _turretLinedUpPublisher;
+    private final BooleanPublisher _shooterShootModePublisher;
+    private final StringPublisher  _shooterModePublisher;
+
+    public Dashboard(Intake intake, Shooter shooter, Turret turret)
+    {
+        _intake  = intake;
+        _shooter = shooter;
+        _turret  = turret;
+
+        var dashboardTable = NetworkTableInstance.getDefault().getTable("Dashboard");
+        var valuesTable    = dashboardTable.getSubTable("Robot Values");
+
+        _matchTimePublisher             = valuesTable.getDoubleTopic("Match Time").publish();
+        _hubActivePublisher             = valuesTable.getBooleanTopic("Hub Active").publish();
+        _hubStateTimeRemainingPublisher = valuesTable.getDoubleTopic("Hub State Time Remaining").publish();
+        _turretHasTargetPublisher       = valuesTable.getBooleanTopic("Turret Has Target").publish();
+        _turretLinedUpPublisher         = valuesTable.getBooleanTopic("Turret Lined Up").publish();
+        _shooterShootModePublisher      = valuesTable.getBooleanTopic("Shooter Is Shoot Mode").publish();
+        _shooterModePublisher           = valuesTable.getStringTopic("Shooter Mode").publish();
+
+        initializeTuningPreferences();
+    }
+
+    @Override
+    public void periodic()
+    {
+        _matchTimePublisher.set(Math.max(DriverStation.getMatchTime(), 0.0));
+
+        boolean hubActive = Utilities.isHubActive();
+        _hubActivePublisher.set(hubActive);
+        _hubStateTimeRemainingPublisher.set(Math.max(Utilities.getHubStateTimeRemaining().in(Seconds), 0.0));
+
+        if (_turret != null)
+        {
+            _turretHasTargetPublisher.set(_turret.hasTarget());
+            _turretLinedUpPublisher.set(_turret.isLinedUp());
+        }
+        else
+        {
+            _turretHasTargetPublisher.set(false);
+            _turretLinedUpPublisher.set(false);
+        }
+
+        HoodPosition shooterMode = _shooter.getShooterMode();
+        _shooterShootModePublisher.set(shooterMode != HoodPosition.Pass);
+        _shooterModePublisher.set(shooterMode.name());
+    }
+
+    private static void initPreference(String key, double value)
+    {
+        Preferences.initDouble(key, value);
+    }
+
+    private void initializeTuningPreferences()
+    {
+        String prefix = "Dashboard/Dashboard Settings/";
+
+        initPreference(prefix + "Intake Forward Voltage", IntakeConstants.INTAKE_VOLTS.in(Volts));
+        initPreference(prefix + "Intake Reverse Voltage", IntakeConstants.REVERSE_VOLTS.in(Volts));
+        initPreference(prefix + "Intake Extension Max Position", IntakeConstants.EXTENSION_MAX_POSITION.in(Inches));
+        initPreference(prefix + "Intake Extend Voltage", IntakeConstants.EXTEND_VOLTS.in(Volts));
+        initPreference(prefix + "Intake Retract Voltage", IntakeConstants.RETRACT_VOLTS.in(Volts));
+
+        initPreference(prefix + "Flywheel kP", ShooterConstants.FLYWHEEL_KP);
+        initPreference(prefix + "Flywheel kD", ShooterConstants.FLYWHEEL_KD);
+        initPreference(prefix + "Flywheel kS", ShooterConstants.FLYWHEEL_KS.in(Volts));
+        initPreference(prefix + "Flywheel kV", ShooterConstants.FLYWHEEL_KV.in(Volts.per(RPM)));
+        initPreference(prefix + "Flywheel kA", ShooterConstants.FLYWHEEL_KA.in(Volts.per(RotationsPerSecondPerSecond)));
+        initPreference(prefix + "Flywheel Tolerance", ShooterConstants.FLYWHEEL_TOLERANCE.in(Value));
+        initPreference(prefix + "Flywheel Pass Velocity", ShooterConstants.PASS_FLYWHEEL_VELOCITY.in(RPM));
+
+        initPreference(prefix + "Hood Min Angle", ShooterConstants.HOOD_MIN_ANGLE.in(Degrees));
+        initPreference(prefix + "Hood Max Angle", ShooterConstants.HOOD_MAX_ANGLE.in(Degrees));
+        initPreference(prefix + "Hood Shoot Angle", ShooterConstants.HOOD_SHOOT_ANGLE.in(Degrees));
+        initPreference(prefix + "Hood Pass Angle", ShooterConstants.HOOD_PASS_ANGLE.in(Degrees));
+        initPreference(prefix + "Hood Tolerance", ShooterConstants.HOOD_TOLERANCE.in(Degrees));
+
+        initPreference(prefix + "Turret kP", ShooterConstants.TURRET_KP);
+        initPreference(prefix + "Turret kI", ShooterConstants.TURRET_KI);
+        initPreference(prefix + "Turret kD", ShooterConstants.TURRET_KD);
+        initPreference(prefix + "Turret Test Setpoint", 0.0);
+        initPreference(prefix + "Turret Min Angle", ShooterConstants.TURRET_MIN_ANGLE.in(Degrees));
+        initPreference(prefix + "Turret Max Angle", ShooterConstants.TURRET_MAX_ANGLE.in(Degrees));
+        initPreference(prefix + "Turret Home Angle", ShooterConstants.TURRET_HOME_ANGLE.in(Degrees));
+        initPreference(prefix + "Turret Pass Angle", ShooterConstants.TURRET_PASS_TARGET.in(Degrees));
+        initPreference(prefix + "Turret Tolerance", ShooterConstants.TURRET_TOLERANCE.in(Degrees));
+
+        initPreference(prefix + "Feeder Voltage", ShooterConstants.FEEDER_VOLTAGE.in(Volts));
+
+        initPreference(prefix + "Vision Max Angular Rate", VisionConstants.MAX_ANGULAR_RATE_FOR_VISION.in(DegreesPerSecond));
+        initPreference(prefix + "Vision Max Tilt", VisionConstants.MAX_TILT_FOR_VISION.in(Degrees));
+
+        initPreference(prefix + "Climber L1 Rotation", ClimberConstants.L1_ROTATION.in(Degrees));
+        initPreference(prefix + "Climber L3 Rotation", ClimberConstants.L3_ROTATION.in(Degrees));
+        initPreference(prefix + "Climber Extension Threshold", ClimberConstants.EXTENSION_THRESHOLD.in(Inches));
+        initPreference(prefix + "Climber Retraction Threshold", ClimberConstants.RETRACTION_THRESHOLD.in(Inches));
+        initPreference(prefix + "Climber Extend Voltage", ClimberConstants.EXTEND_OUTPUT.in(Volts));
+        initPreference(prefix + "Climber Retract Voltage", ClimberConstants.RETRACT_VOLTAGE.in(Volts));
+        initPreference(prefix + "Climber Rotate Voltage", ClimberConstants.ROTATE_OUTPUT.in(Volts));
+        initPreference(prefix + "Climber Tolerance", ClimberConstants.ROTATION_TOLERANCE.in(Degrees));
+    }
+}

src/main/java/frc/robot/subsystems/shooter/Flywheel.java

@@ -24,6 +24,7 @@
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.simulation.FlywheelSim;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
@@ -47,6 +48,11 @@ public class Flywheel
     private AngularVelocity                 _targetVelocity  = RPM.zero();
     @Logged
     private Voltage                         _flywheelVoltage = Volts.zero();
+    private double                          _lastKP;
+    private double                          _lastKD;
+    private double                          _lastKS;
+    private double                          _lastKV;
+    private double                          _lastKA;
 
     public Flywheel()
     {
@@ -71,6 +77,12 @@ public Flywheel()
         _closedLoopController = _leadMotor.getClosedLoopController();
         _flywheelEncoder      = _leadMotor.getEncoder();
 
+        _lastKP = ShooterConstants.FLYWHEEL_KP;
+        _lastKD = ShooterConstants.FLYWHEEL_KD;
+        _lastKS = ShooterConstants.FLYWHEEL_KS.in(Volts);
+        _lastKV = ShooterConstants.FLYWHEEL_KV.in(Volts.per(RPM));
+        _lastKA = ShooterConstants.FLYWHEEL_KA.in(Volts.per(RotationsPerSecondPerSecond));
+
         if (RobotBase.isReal())
         {
             _leadMotor.setCANTimeout(0);
@@ -90,6 +102,24 @@ public void periodic()
     {
         _velocity        = RPM.of(_flywheelEncoder.getVelocity());
         _flywheelVoltage = Volts.of(_leadMotor.getAppliedOutput() * _leadMotor.getBusVoltage());
+
+        double newP = Preferences.getDouble("Dashboard/Dashboard Settings/Flywheel kP", ShooterConstants.FLYWHEEL_KP);
+        double newD = Preferences.getDouble("Dashboard/Dashboard Settings/Flywheel kD", ShooterConstants.FLYWHEEL_KD);
+        double newS = Preferences.getDouble("Dashboard/Dashboard Settings/Flywheel kS", ShooterConstants.FLYWHEEL_KS.in(Volts));
+        double newV = Preferences.getDouble("Dashboard/Dashboard Settings/Flywheel kV", ShooterConstants.FLYWHEEL_KV.in(Volts.per(RPM)));
+        double newA = Preferences.getDouble("Dashboard/Dashboard Settings/Flywheel kA", ShooterConstants.FLYWHEEL_KA.in(Volts.per(RotationsPerSecondPerSecond)));
+        if (newP != _lastKP || newD != _lastKD || newS != _lastKS || newV != _lastKV || newA != _lastKA)
+        {
+            var config = new SparkFlexConfig();
+            config.closedLoop.p(newP).d(newD);
+            config.closedLoop.feedForward.kS(newS).kV(newV).kA(newA);
+            _leadMotor.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);
+            _lastKP = newP;
+            _lastKD = newD;
+            _lastKS = newS;
+            _lastKV = newV;
+            _lastKA = newA;
+        }
     }
 
     public void simulationPeriodic()