add turret offset back

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index f5a11f52d018dce42f0a9234ad93da7e8dad6eca..b370ae4894397a616d556f59c6773d4a23b95e7f 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -63,7 +63,7 @@ public class Superstructure extends Command {
 
     private LoggedNetworkNumber hoodOffset = new LoggedNetworkNumber("OPERATOR: Hood Offset", 0.0);
 
-    private LoggedNetworkNumber turretOffset = new LoggedNetworkNumber("OPERATOR: Turret Offset", 0.0);
+    private double turretOffset = 0.0;
 
     private double distanceFromTarget = 0.0;
 
@@ -166,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.get();
+        double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error + turretOffset;
 
         // Stay within physical limits -- if shortest path is past max angle, we go long way around
         if (potentialSetpoint > TurretConstants.MAX_ANGLE) {
@@ -231,12 +231,12 @@ public class Superstructure extends Command {
 
     // aim more left
     public void bumpUpTurretOffset() {
-        turretOffset.set(turretOffset.get() + 2.5); //2.5 deg
+        turretOffset += 2.5; //2.5 deg
     }
 
     // aim more right
     public void bumpDownTurretOffset() {
-        turretOffset.set(turretOffset.get() - 2.5); //2.5 deg
+        turretOffset -= 2.5; //2.5 deg
     }
 
     @Override
@@ -248,6 +248,9 @@ public class Superstructure extends Command {
         updateDrivePose();
         updateSetpoints(drivepose);
 
+        turretOffset = SmartDashboard.getNumber("OPERATOR: Turret Offset", turretOffset);
+        SmartDashboard.putNumber("OPERATOR: Turret Offset", turretOffset);
+
         if (phaseManager.isIdle()) {
             underLadder();
         } else {