From d0897024efb037cf9a33c2ea21012c52e21ab668 Mon Sep 17 00:00:00 2001 From: WesleyWong-972 Date: Fri, 24 Apr 2026 18:39:07 -0700 Subject: [PATCH] sending to houston now adding minimum wait for dynamic, spindexer jamming should be more accurate, drive offsets redone --- src/main/java/frc/robot/RobotContainer.java | 7 +++++-- .../java/frc/robot/commands/gpm/RunSpindexer.java | 2 +- .../robot/commands/gpm/RunSpindexerWithStop.java | 13 ++++++++++++- .../frc/robot/constants/swerve/DriveConstants.java | 8 ++++---- .../frc/robot/subsystems/spindexer/Spindexer.java | 10 +++++++++- .../subsystems/spindexer/SpindexerConstants.java | 5 +++-- 6 files changed, 34 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 94b0c42..87b7731 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -65,7 +65,7 @@ public class RobotContainer { private Hood hood = null; private Spindexer spindexer = null; private Intake intake = null; - private LED led = null; + // private LED led = null; // Controllers are defined here private BaseDriverConfig driver = null; @@ -107,7 +107,7 @@ public class RobotContainer { case PrimeJr: // AKA Valence spindexer = new Spindexer(); intake = new Intake(); - led = new LED(); + // led = new LED(); breaker = new EMABreaker(); case WaffleHouse: // AKA Betabot @@ -156,6 +156,7 @@ public class RobotContainer { } break; } + if (intake != null && hood != null && turret != null) // CommandScheduler.getInstance().schedule(new HardstopWarning(hood, intake, turret)); (no more crt for this) @@ -315,6 +316,8 @@ public class RobotContainer { String rightDynamicLiberalDoubleSwipe = "RightDynamicDoubleLiberalSwipe"; String leftDynamicConservativeDoubleSwipe = "LeftDynamicDoubleConservativeSwipe"; String rightDynamicConservativeDoubleSwipe = "RightDynamicDoubleConservativeSwipe"; + String leftDynamicShallowDoubleSwipe = "LeftDynamicShallowDoubleSwipe"; + String rightDynamicShallowDoubleSwipe = "RightDynamicShallowDoubleSwipe"; // add commands addAuto(leftDynamicLiberalDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleLiberalSwipe(true)); diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java index 9bd1550..f2ba4ba 100644 --- a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java +++ b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java @@ -63,7 +63,7 @@ public class RunSpindexer extends Command { reversing = false; return; // this is so the balls don't fly out when unaligned } - boolean jammed = spindexer.getSubsystemStatorCurrent() / 2 > SpindexerConstants.JAM_CURRENT_THRESHOLD; + boolean jammed = spindexer.getMotorOneVelocity() < SpindexerConstants.JAM_CURRENT_THRESHOLD && spindexer.getMotorOneStatorCurrent() > SpindexerConstants.JAM_CURRENT_THRESHOLD; Logger.recordOutput("SpindexerJammed", jammed); if (jam_debouncer.calculate(jammed)) { Logger.recordOutput("SpindexerJammedDebounced", jammed); diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java b/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java index 6a00feb..742a0fb 100644 --- a/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java +++ b/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java @@ -35,6 +35,8 @@ public class RunSpindexerWithStop extends Command { private Debouncer debouncer = new Debouncer(0.3, DebounceType.kRising); + private final double interval = 0.6; // Change this to make it faster/slower (seconds) + public RunSpindexerWithStop(Spindexer spindexer, Turret turret, Hood hood, Intake intake) { this.spindexer = spindexer; this.turret = turret; @@ -95,16 +97,25 @@ public class RunSpindexerWithStop extends Command { if (!Constants.DISABLE_SMART_DASHBOARD) { SmartDashboard.putBoolean("Spindexer Jamming", reversing); } + + if ((int) (Timer.getFPGATimestamp() / interval) % 2 == 0) { + intake.extend(); + } else { + intake.intermediateExtend(); + } + } @Override public void end(boolean interrupted) { spindexer.stopSpindexer(); reversing = false; + + intake.extend(); } @Override public boolean isFinished() { - return runTimer.hasElapsed(1.0) && debouncer.calculate(spindexer.spinningAir()); + return (runTimer.hasElapsed(1.0) && debouncer.calculate(spindexer.spinningAir())) || runTimer.hasElapsed(4.0); } } diff --git a/src/main/java/frc/robot/constants/swerve/DriveConstants.java b/src/main/java/frc/robot/constants/swerve/DriveConstants.java index 4f10642..d136d10 100644 --- a/src/main/java/frc/robot/constants/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/constants/swerve/DriveConstants.java @@ -208,10 +208,10 @@ public class DriveConstants { */ public static void update(RobotId robotId) { if (robotId == RobotId.PrimeJr) { - STEER_OFFSET_FRONT_LEFT = 188.26+180; - STEER_OFFSET_FRONT_RIGHT = 162.71+180+180; - STEER_OFFSET_BACK_LEFT = 196.69+180; - STEER_OFFSET_BACK_RIGHT = 357.90+180+180; + STEER_OFFSET_FRONT_LEFT = 187.64+180; // module zero + STEER_OFFSET_FRONT_RIGHT = 162+180+180; // module one + STEER_OFFSET_BACK_LEFT = 196.3+180; // module two + STEER_OFFSET_BACK_RIGHT = 357+180+180; // module three // MK5n INVERT_STEER_MOTOR = InvertedValue.CounterClockwise_Positive; diff --git a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java index 2f32fe1..e4bcceb 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/spindexer/Spindexer.java @@ -2,6 +2,7 @@ package frc.robot.subsystems.spindexer; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; @@ -89,7 +90,6 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { } Logger.recordOutput("HasBalls", spinningAir()); - } public void setMotorVoltages(double voltage) { @@ -144,6 +144,14 @@ public class Spindexer extends SubsystemBase implements SpindexerIO { return getMotorOneStatorCurrent() < 16.0 && getMotorTwoStatorCurrent() < 28.0; } + public double getMotorOneVelocity() { + return inputs.spindexerOneVelocity; + } + + public double getMotorTwoVelocity() { + return inputs.spindexerTwoVelocity; + } + @Override public void updateInputs() { inputs.spindexerOneVelocity = motorOne.getVelocity().getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java index e4d5708..54b026e 100644 --- a/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java +++ b/src/main/java/frc/robot/subsystems/spindexer/SpindexerConstants.java @@ -2,7 +2,7 @@ package frc.robot.subsystems.spindexer; public class SpindexerConstants { public static final double spindexerVelocityWithBall = 6.0; // rps (for counting balls) - public static final double SUPPLY_CURRENT_LIMIT = 40; // A + public static final double SUPPLY_CURRENT_LIMIT = 80; // A public static final double spindexerForwardVoltage = 1.00; // Volts (set low for testing) public static final double spindexerReverseVoltage = -1.00; // Volts public static final double GEAR_RATIO = 27.0; // unused & both motors have same gearing @@ -10,7 +10,8 @@ public class SpindexerConstants { public static final double CURRENT_FORWARD_STATOR_LIMIT = 150.0; public static final double CURRENT_REVERSE_STATOR_LIMIT = 20.0; public static final double CURRENT_TIME_LIMIT = 1.0; //s - public static final double JAM_CURRENT_THRESHOLD = 40.0; // A + public static final double JAM_CURRENT_THRESHOLD = 10.0; // A + public static final double JAM_VELOCITY_THRESHOLD = 10.0; // A public static final double JAM_DEBOUNCE_TIME = 0.3; // seconds public static final double REVERSE_DEBOUNCE_TIME = 0.25; // seconds } -- 2.39.5