netowrk numbies

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index 1fe654d47b0fb0d5610aefc7bf117601c29517a5..854db9ec85afd8a7970cfd7a53d4f512b00dced4 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -1,6 +1,7 @@
 package frc.robot.commands.gpm;
 
 import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.LinearFilter;
@@ -14,7 +15,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.constants.Constants;
 import frc.robot.constants.FieldConstants;
 import frc.robot.constants.ShotInterpolation;
@@ -61,14 +61,15 @@ public class Superstructure extends Command {
 
     private PhaseManager phaseManager = new PhaseManager();
 
-    private double hoodOffset = 0.0;
+    private LoggedNetworkNumber hoodOffset = new LoggedNetworkNumber("OPERATOR: Hood Offset", 0.0);
 
-    private double turretOffset = 0.0;
+    private LoggedNetworkNumber turretOffset = new LoggedNetworkNumber("OPERATOR: Turret Offset", 0.0);
 
     private double distanceFromTarget = 0.0;
 
     // private double TOFAdjustment = 0.85;
-    private double TOFAdjustment = 1.1;
+    // private double TOFAdjustment = 1.1;
+    private LoggedNetworkNumber TOFAdjustment = new LoggedNetworkNumber("OPERATOR: TOF Adjustment", 1.1);
 
     public Superstructure(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) {
         this.turret = turret;
@@ -122,7 +123,7 @@ public class Superstructure extends Command {
             target3d.minus(lookahead3d),
             2.0);
 
-            timeOfFlight = goalState.timeOfFlight() * TOFAdjustment;
+            timeOfFlight = goalState.timeOfFlight() * TOFAdjustment.get();
             double offsetX = turretVelocityX * timeOfFlight;
             double offsetY = turretVelocityY * timeOfFlight;
             Pose2d newLookaheadPose =
@@ -165,7 +166,7 @@ public class Superstructure extends Command {
 
         // Shortest path
         double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180);
-        double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error + turretOffset;
+        double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error + turretOffset.get();
 
         // Stay within physical limits -- if shortest path is past max angle, we go long way around
         if (potentialSetpoint > TurretConstants.MAX_ANGLE) {
@@ -220,35 +221,26 @@ public class Superstructure extends Command {
 
     // shoot higher
     public void bumpUpHoodOffset() {
-        hoodOffset += 1.0; // 1 degree
+        hoodOffset.set(hoodOffset.get() + 1.0); //1 deg
     }
 
     // shoot lower
     public void bumpDownHoodOffset() {
-        hoodOffset -= 1.0; // 1 degree
+        hoodOffset.set(hoodOffset.get() - 1.0); //1 deg
     }
 
     // aim more left
     public void bumpUpTurretOffset() {
-        turretOffset += 2.5; // 2.5 degree
+        turretOffset.set(turretOffset.get() + 2.5); //2.5 deg
     }
+
     // aim more right
     public void bumpDownTurretOffset() {
-        turretOffset -= 2.5; // 2.5 degree
+        turretOffset.set(turretOffset.get() - 2.5); //2.5 deg
     }
 
     @Override
     public void execute() {
-        if (!Constants.DISABLE_SMART_DASHBOARD) {
-            TOFAdjustment = SmartDashboard.getNumber("OPERATOR: TOF Adjustment", TOFAdjustment);
-            SmartDashboard.putNumber("OPERATOR: TOF Adjustment", TOFAdjustment);
-            hoodOffset = SmartDashboard.getNumber("OPERATOR: Hood Offset", hoodOffset);
-            SmartDashboard.putNumber("OPERATOR: Hood Offset", hoodOffset);
-        }
-
-        turretOffset = SmartDashboard.getNumber("OPERATOR: Turret Offset", turretOffset);
-        SmartDashboard.putNumber("OPERATOR: Turret Offset", turretOffset);
-
         // If we ever need to lose a match use this code below
         // SmartDashboard.putData("Hood: Shoot Higher", new InstantCommand(() -> bumpUpHoodOffset()));
         // SmartDashboard.putData("Hood: Shoot Lower", new InstantCommand(() -> bumpDownHoodOffset()));