diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 7d7260dd..3390cfe8 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -5,6 +5,19 @@ /***************************************************************/ 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; @@ -12,10 +25,13 @@ 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,9 +41,9 @@ 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; @@ -35,16 +51,6 @@ 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); diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java index 87137ee7..4c23fd96 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/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()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java index e45933cd..20f5e471 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java +++ b/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()); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java index 97c312cf..bc5182e5 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/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,9 +135,7 @@ 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(); @@ -137,7 +143,7 @@ public void periodicAfterScheduler() { 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()) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerSim.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerSim.java index 61b568db..75345e41 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerSim.java +++ b/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 sim; @@ -107,9 +107,7 @@ 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(); @@ -117,7 +115,7 @@ public void periodic() { 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); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java index 9a976593..24869c41 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java +++ b/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(); diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java index 37595df2..e902fac9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java @@ -118,12 +118,19 @@ private void setVoltageOverride(Optional 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; }