a

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index b22ab22c2597c1dc5502cefdacdd92e02ac1cfab..35478b047cd36d20dcfb25e207adafcefd7889d8 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -148,7 +148,7 @@ public class RobotContainer {
           e.printStackTrace();
         }
         if(turret != null){
-          //turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer));
+          turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer));
         }
         drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
         break;

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index 7e55227588da0c2f2fc1109fa01c8edc2d5db85a..efdaa72869a6b19b867620c926d300712b7d59ce 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -60,13 +60,14 @@ public class Superstructure extends Command {
 
     private final double phaseDelay = 0.03; // Extrapolation delay due to latency
 
-    //TODO: Change this back
     private Translation2d target = FieldConstants.HUB_BLUE.toTranslation2d();
 
     private PhaseManager phaseManager = new PhaseManager();
 
     private double hoodOffset = 0.0;
 
+    private double turretOffset = 0.0;
+
     public Superstructure(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) {
         this.turret = turret;
         this.drivetrain = drivetrain;
@@ -158,7 +159,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;
+        double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error + turretOffset;
 
         // Stay within +/- 200 -- if  shortest path is past 200, we go long way around
         double turretRange = TurretConstants.MAX_ANGLE - TurretConstants.MIN_ANGLE;
@@ -210,6 +211,8 @@ public class Superstructure extends Command {
     public void execute() {
         hoodOffset = SmartDashboard.getNumber("Hood Offset", hoodOffset);
         SmartDashboard.putNumber("Hood Offset", hoodOffset);
+        turretOffset = SmartDashboard.getNumber("Turret Offset", turretOffset);
+        SmartDashboard.putNumber("Turret Offset", turretOffset);
         // Phase manager stuff
         phaseManager.update(drivepose, shooter, turret);
         target = phaseManager.getTarget();