Refactor/no ref/sotm becomes fotm

src/main/java/com/stuypulse/robot/Robot.java

@@ -5,17 +5,33 @@
 /***************************************************************/
 package com.stuypulse.robot;
 
+import java.lang.management.GarbageCollectorMXBean;
+import java.lang.management.ManagementFactory;
+import java.lang.reflect.Field;
+import java.util.List;
+import java.util.Timer;
+
+import com.ctre.phoenix6.SignalLogger;
+import com.pathplanner.lib.commands.FollowPathCommand;
+import com.pathplanner.lib.commands.PathfindingCommand;
+import com.stuypulse.robot.commands.handoff.HandoffStop;
+import com.stuypulse.robot.commands.spindexer.SpindexerStop;
+import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM;
+import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM;
 import com.stuypulse.robot.commands.swerve.SwerveAutonInit;
 import com.stuypulse.robot.commands.swerve.SwerveTeleopInit;
 import com.stuypulse.robot.commands.vision.SetMegaTagMode;
 import com.stuypulse.robot.commands.vision.WhitelistAllTagsForAllCameras;
 import com.stuypulse.robot.commands.vision.WhitelistRoutineLeftSideAuto;
 import com.stuypulse.robot.commands.vision.WhitelistRoutineRightSideAuto;
 import com.stuypulse.robot.constants.Cameras;
-import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.constants.Cameras.Camera;
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.subsystems.handoff.HandoffImpl;
+import com.stuypulse.robot.subsystems.spindexer.Spindexer;
 import com.stuypulse.robot.subsystems.superstructure.Superstructure;
 import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState;
+import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;
 import com.stuypulse.robot.subsystems.vision.LimelightVision;
 import com.stuypulse.robot.util.EnergyUtil;
 import com.stuypulse.robot.util.PhoenixUtil;
@@ -25,26 +41,16 @@
 import edu.wpi.first.net.PortForwarder;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.IterativeRobotBase;
 import edu.wpi.first.wpilibj.RobotController;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.Watchdog;
 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.CommandScheduler;
 
-import java.lang.management.GarbageCollectorMXBean;
-import java.lang.management.ManagementFactory;
-import java.lang.reflect.Field;
-import java.util.List;
-import java.util.Timer;
-
-import com.ctre.phoenix6.SignalLogger;
-import com.pathplanner.lib.commands.FollowPathCommand;
-import com.pathplanner.lib.commands.PathfindingCommand;
-
 public class Robot extends TimedRobot {
 
     public enum RobotMode {
@@ -109,7 +115,6 @@ public void robotInit() {
         } catch (Exception e) {
             DriverStation.reportError("Failed to disable loop overrun warnings.", e.getStackTrace());
         }
-        // this doesnt seem to work? 3/25 11:46AM
 
         DataLogManager.start();
         SignalLogger.start();
@@ -119,19 +124,6 @@ public void robotInit() {
         energyUtil = new EnergyUtil();
 
         CommandScheduler.getInstance().schedule(new SwerveAutonInit());
-
-        // threadTimer = new Timer();
-        // shouldRunSecondThread = new SmartBoolean("Robot/Run second Thread", true);
-        // threadTimer.scheduleAtFixedRate(new TimerTask() {
-        //     @Override
-        //     public void run() {
-        //         if (shouldRunSecondThread.get()) {
-        //             for (int i = 0; i < 1000; i++) {
-        //                 System.out.println("second thread!" + (Math.cos(edu.wpi.first.wpilibj.Timer.getFPGATimestamp() * i)));
-        //             }
-        //         }
-        //     }
-        // },0,  1);
     }
 
     @Override
@@ -168,6 +160,14 @@ public void robotPeriodic() {
             alliance = DriverStation.getAlliance().get();
         }
 
+        if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && Superstructure.getInstance().getState() == SuperstructureState.SOTM) {
+            CommandScheduler.getInstance().schedule(
+                    new SuperstructureFOTM(),
+                    new SpindexerStop(),
+                    new HandoffStop()
+            );
+        }
+
         SmartDashboard.putNumber("Robot/Match Time", DriverStation.getMatchTime());
         SmartDashboard.putData("Robot/Scheduled Commands", CommandScheduler.getInstance());
         SmartDashboard.putNumber("Robot/Battery Voltage", batteryVoltage);

src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java

@@ -5,8 +5,16 @@
 /***************************************************************/
 package com.stuypulse.robot.subsystems.handoff;
 
-import com.stuypulse.stuylib.streams.booleans.BStream;
-import com.stuypulse.stuylib.streams.booleans.filters.BDebounce;
+import java.util.Optional;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.MotorAlignmentValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.stuypulse.robot.Robot;
 import com.stuypulse.robot.Robot.RobotMode;
 import com.stuypulse.robot.RobotContainer.EnabledSubsystems;
@@ -19,6 +27,8 @@
 import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;
 import com.stuypulse.robot.util.PhoenixUtil;
 import com.stuypulse.robot.util.SysId;
+import com.stuypulse.stuylib.streams.booleans.BStream;
+import com.stuypulse.stuylib.streams.booleans.filters.BDebounce;
 
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
@@ -27,16 +37,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 
-import com.ctre.phoenix6.BaseStatusSignal;
-import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.controls.DutyCycleOut;
-import com.ctre.phoenix6.controls.Follower;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.MotorAlignmentValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-import java.util.Optional;
-
 public class HandoffImpl extends Handoff {
     private final Motors.TalonFXConfig handoffConfig;
 
@@ -147,14 +147,12 @@ public boolean shouldStop() {
     public void periodicAfterScheduler() {
         super.periodicAfterScheduler();
 
-        boolean shouldNotShootIntoHub = (Superstructure.getInstance().superstructureInShootIntoHubMode()) ? 
-            !CommandSwerveDrivetrain.getInstance().canShootIntoHub() 
-            : false;
+        // removed shouldNotShootIntoHub logic (no longer used)
 
         if (EnabledSubsystems.HANDOFF.get() && getState() != HandoffState.STOP) {
             if (voltageOverride.isPresent()) {
                 motorLead.setVoltage(voltageOverride.get());
-            } else if (shouldStop() || shouldNotShootIntoHub) {
+            } else if (shouldStop()) {
                 motorLead.stopMotor();
                 motorFollow.stopMotor();
             } else {
@@ -169,10 +167,8 @@ public void periodicAfterScheduler() {
         SmartDashboard.putBoolean("Handoff/ShouldStop?", shouldStop());
         SmartDashboard.putNumber("Handoff/Lead Velocity", getLeaderRPM());
         SmartDashboard.putNumber("Handoff/Follow Velocity", getLeaderRPM());
-        SmartDashboard.putBoolean("Handoff/Should Not Shoot Into Hub", shouldNotShootIntoHub);
-
         SmartDashboard.putBoolean("Spindexer/Should Stop", shouldStop());
-        SmartDashboard.putBoolean("Spindexer/Should Not Shoot Into Hub", shouldNotShootIntoHub);
+
 
         if (Settings.DEBUG_MODE.get()) {     
             SmartDashboard.putNumber("Handoff/Lead Voltage", motorLeadVoltage.getValueAsDouble());

src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java

@@ -103,15 +103,13 @@ public void periodic() {
         controller.correct(VecBuilder.fill(sim.getOutput(0)));
         controller.predict(Settings.DT);
 
-        boolean shouldNotShootIntoHub = (Superstructure.getInstance().superstructureInShootIntoHubMode()) ? 
-            !CommandSwerveDrivetrain.getInstance().canShootIntoHub() 
-            : false;
+        // removed shouldNotShootIntoHub logic (no longer used)
 
         if (EnabledSubsystems.HANDOFF.get()) {
             if (voltageOverride.isPresent()) {
                 sim.setInput(voltageOverride.get());
                 SmartDashboard.putNumber("Handoff/Input Voltage", voltageOverride.get());
-            } else if (shouldStop() || shouldNotShootIntoHub) {
+            } else if (shouldStop()) {
                 sim.setInput(0.0);
                 SmartDashboard.putNumber("Handoff/Input Voltage", 0.0);
             } else {
@@ -125,8 +123,7 @@ public void periodic() {
 
         sim.update(Settings.DT);
 
-        SmartDashboard.putBoolean("Handoff/Should Stop", shouldStop());
-        SmartDashboard.putBoolean("Handoff/Should Not Shoot Into Hub", shouldNotShootIntoHub);
+    SmartDashboard.putBoolean("Handoff/Should Stop", shouldStop());
         SmartDashboard.putNumber("Handoff/Target Duty Cycle", getTargetDutyCycle());
     }
 

src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java

@@ -5,8 +5,13 @@
 /***************************************************************/
 package com.stuypulse.robot.subsystems.spindexer;
 
-import com.stuypulse.stuylib.streams.booleans.BStream;
-import com.stuypulse.stuylib.streams.booleans.filters.BDebounce;
+import java.util.Optional;
+
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.stuypulse.robot.Robot;
 import com.stuypulse.robot.Robot.RobotMode;
 import com.stuypulse.robot.RobotContainer.EnabledSubsystems;
@@ -20,6 +25,8 @@
 import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;
 import com.stuypulse.robot.util.PhoenixUtil;
 import com.stuypulse.robot.util.SysId;
+import com.stuypulse.stuylib.streams.booleans.BStream;
+import com.stuypulse.stuylib.streams.booleans.filters.BDebounce;
 
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
@@ -29,6 +36,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 
+
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.controls.DutyCycleOut;
 import com.ctre.phoenix6.controls.VelocityVoltage;
@@ -127,17 +135,15 @@ private boolean spindexerUnjam() {
     public void periodicAfterScheduler() {
         super.periodicAfterScheduler();
 
-        boolean shouldNotShootIntoHub = (Superstructure.getInstance().superstructureInShootIntoHubMode()) ? 
-            !CommandSwerveDrivetrain.getInstance().canShootIntoHub() 
-            : false;
+        // removed shouldNotShootIntoHub logic (no longer used)
 
         // boolean unJamming = spindexerUnjam();
 
         if (EnabledSubsystems.SPINDEXER.get()) {
             if (voltageOverride.isPresent()) {
                 leaderMotor.setVoltage(voltageOverride.get());
             } else {
-                if ((shouldStop() || shouldNotShootIntoHub)) {
+                if (shouldStop()) {
                     leaderMotor.stopMotor();
                 }             
                 else {
@@ -155,8 +161,7 @@ public void periodicAfterScheduler() {
         SmartDashboard.putNumber("Spindexer/Leader Supply Current (amps)", leaderSupplyCurrent.getValueAsDouble());
         SmartDashboard.putNumber("Spindexer/Leader Stator Current (amps)", leaderStatorCurrent.getValueAsDouble());
 
-        SmartDashboard.putBoolean("Spindexer/Should Stop?", shouldStop());
-        SmartDashboard.putBoolean("Spindexer/shouldNotShootIntoHub", shouldNotShootIntoHub);
+    SmartDashboard.putBoolean("Spindexer/Should Stop?", shouldStop());
 
         if (Settings.DEBUG_MODE.get()) {
             if (Robot.getMode() == RobotMode.DISABLED && !DriverStation.isFMSAttached()) {

src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerSim.java

@@ -5,6 +5,8 @@
 /***************************************************************/
 package com.stuypulse.robot.subsystems.spindexer;
 
+import java.util.Optional;
+
 import com.stuypulse.robot.RobotContainer.EnabledSubsystems;
 import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.subsystems.handoff.Handoff;
@@ -27,8 +29,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 
-import java.util.Optional;
-
 public class SpindexerSim extends Spindexer {
 
     private LinearSystemSim<N1, N1, N1> sim;
@@ -107,17 +107,15 @@ public void periodic() {
         controller.correct(VecBuilder.fill(sim.getOutput(0)));
         controller.predict(Settings.DT);
 
-        boolean shouldNotShootIntoHub = (Superstructure.getInstance().superstructureInShootIntoHubMode()) ? 
-            !CommandSwerveDrivetrain.getInstance().canShootIntoHub() 
-            : false;
+        // removed shouldNotShootIntoHub logic (no longer used)
 
         boolean isUnjamming = spindexerUnjam();
 
         if (EnabledSubsystems.SHOOTER.get()) {
             if (voltageOverride.isPresent()) {
                 sim.setInput(voltageOverride.get());
                 SmartDashboard.putNumber("Spindexer/Input Voltage", voltageOverride.get());
-            } else if ((shouldStop() || shouldNotShootIntoHub) && !isUnjamming) {
+            } else if (shouldStop() && !isUnjamming) {
                 sim.setInput(0);
             } else {
                 SmartDashboard.putNumber("Spindexer/Input Voltage", controller.getU(0));
@@ -130,7 +128,6 @@ public void periodic() {
 
         SmartDashboard.putNumber("Spindexer/Current RPM", getCurrentRPM());
         SmartDashboard.putBoolean("Spindexer/Should Stop", shouldStop());
-        SmartDashboard.putBoolean("Spindexer/Should Not Shoot Into Hub", shouldNotShootIntoHub);
         SmartDashboard.putBoolean("Spindexer/Unjamming", isUnjamming);
 
         sim.update(Settings.DT);

src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java

@@ -147,18 +147,6 @@ public double getCurrentDraw() {
         return turret.getCurrentDraw() + shooter.getCurrentDraw() + hood.getCurrentDraw();
     }
 
-    public boolean superstructureInShootIntoHubMode() {
-        return (state == SuperstructureState.AUTO_INTERPOLATION || 
-                state == SuperstructureState.AUTO_INTERPOLATION_SOTM ||
-                state == SuperstructureState.INTERPOLATION ||
-                state == SuperstructureState.MANUAL_OVERRIDE ||
-                state == SuperstructureState.SOTM ||
-                state == SuperstructureState.KB ||
-                state == SuperstructureState.LEFT_CORNER ||
-                state == SuperstructureState.RIGHT_CORNER ||
-                state == SuperstructureState.STOW);
-    }
-
     public void periodicAfterScheduler() {
         SuperstructureState state = getState();
 

src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java

@@ -118,12 +118,19 @@ private void setVoltageOverride(Optional<Double> volts) {
 
     private double getDelta(double target, double current) {
         double delta = (target - current) % 360;
-
-        if (delta > 180.0) delta -= 360;
-        else if (delta < -180) delta += 360;
 
-        if (current + delta < Settings.Superstructure.Turret.RANGE_CW) return delta + 360;
-        if (current + delta > Settings.Superstructure.Turret.RANGE_CCW) return delta - 360;
+        if (delta > 180.0) {
+            delta -= 360;
+        } else if (delta < -180) {
+            delta += 360;
+        }
+
+        if (current + delta > Settings.Superstructure.Turret.RANGE_CW) {
+            return delta - 360;
+        }
+        if (current + delta < Settings.Superstructure.Turret.RANGE_CCW) {
+            return delta + 360;
+        }
 
         return delta;
     }