Tuning Mode

.vscode/settings.json

@@ -1,4 +1,5 @@
 {
     "C_Cpp.errorSquiggles": "disabled",
-    "java.configuration.updateBuildConfiguration": "interactive"
+    "java.configuration.updateBuildConfiguration": "interactive",
+    "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
 }

src/main/java/frc/robot/RobotState.java

@@ -32,6 +32,7 @@ public enum State {
     INTAKING,
     NOTE_HELD,
     TELEOP,
+    TUNING,
     IDLE
   }
 
@@ -148,6 +149,10 @@ public void updateState() {
         break;
       case TELEOP:
         break;
+      case TUNING:
+        intake.setState(IntakeState.TUNING);
+        shooter.setState(ShooterState.TUNING);
+        drive.setState(DriveState.TUNING);
       default:
         break;
     }

src/main/java/frc/robot/subsystems/Drivetrain/Drivetrain.java

@@ -1,6 +1,7 @@
 package frc.robot.subsystems.Drivetrain;
 
 import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -32,6 +33,11 @@ public enum DriveState {
   private final SwerveIOInputsAutoLogged swerveIOInputs = new SwerveIOInputsAutoLogged();
   private final Limelight m_limelight;
 
+  private LoggedDashboardNumber vxMetersPerSecond = new LoggedDashboardNumber("Drivetrain/Tuning/vxMetersPerSecond");
+  private LoggedDashboardNumber vyMetersPerSecond = new LoggedDashboardNumber("Drivetrain/Tuning/vyMetersPerSecond");
+  private LoggedDashboardNumber omegaRadiansPerSecond = new LoggedDashboardNumber("Drivetrain/Tuning/vxMetersPerSecond");
+
+
   private DriveState state = DriveState.MANUAL;
 
   private ChassisSpeeds driverChassisSpeeds = new ChassisSpeeds(); // Robot Relative
@@ -88,7 +94,6 @@ public void periodic() {
         }
 
         // Intentional Fall-through - if Limelight does not detect target, we do manual driving
-      case TUNING:
       case MANUAL:
         m_swerveDrivetrainIO.drive(driverChassisSpeeds, false, speedBoost);
         // m_swerveDrivetrainIO.drive(driveX, driveY, driveOmega, true, false, speedBoost);
@@ -147,6 +152,8 @@ public void periodic() {
             false,
             speedBoost);
       break;
+      case TUNING:
+        setDriverDesired(new ChassisSpeeds(vxMetersPerSecond.get(), vyMetersPerSecond.get(), omegaRadiansPerSecond.get()));
     }
 
     // Logger.recordOutput("Drive/DrivetrainState", state.toString());

src/main/java/frc/robot/subsystems/Intake/Intake.java

@@ -4,10 +4,11 @@
 import frc.robot.subsystems.Intake.Roller.RollerIO;
 import frc.robot.subsystems.Intake.Roller.RollerIOInputsAutoLogged;
 import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
 public class Intake extends SubsystemBase {
-
   private RollerIO rollerIO;
+  private LoggedDashboardNumber rollerSpeed = new LoggedDashboardNumber("Intake/Tuning/rollerSpeed");
   private RollerIOInputsAutoLogged rollerIOInputs = new RollerIOInputsAutoLogged();
 
   public enum IntakeState {
@@ -46,7 +47,7 @@ public void periodic() {
         rollerIO.setSpeed(-1);
         break;
       case TUNING:
-        // TODO
+        rollerIO.setSpeed(rollerSpeed.get());
         break;
       case NOTE_HELD:
       case IDLE: