From b60b7bcb621fbd01091499c8eec22f07b4d8d95e Mon Sep 17 00:00:00 2001 From: Fatblabs Date: Sun, 5 Apr 2026 00:29:35 -0700 Subject: [PATCH 1/4] Ball counter --- .../robot/subsystems/launcher/Flywheels.java | 70 ++++++++++++++++++- 1 file changed, 67 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/Flywheels.java b/src/main/java/frc/robot/subsystems/launcher/Flywheels.java index 04c16c977..98939f8ec 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Flywheels.java +++ b/src/main/java/frc/robot/subsystems/launcher/Flywheels.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.launcher; +import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.Follower; @@ -10,9 +11,13 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.networktables.DoublePublisher; import edu.wpi.first.networktables.DoubleTopic; +import edu.wpi.first.networktables.IntegerPublisher; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.TimestampedDouble; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -35,13 +40,27 @@ public class Flywheels extends SubsystemBase { new Follower(Hardware.FLYWHEEL_TWO_ID, MotorAlignmentValue.Opposed); public NtTunableDouble targetVelocity; + private double lastRPS = 0; private long lastPositionUpdateTime = 0; + private final double minDerivative = -2; + private final double maxDerivative = 2; + private IntegerPublisher ballPub; + private int ballCount = 0; public final double FLYWHEEL_TOLERANCE = 15; // RPS // increased on drive practice 3/18 from 5 -> 10 //Increased to 15 by TD 3/18 public final NtTunableBoolean TUNER_CONTROLLED = new NtTunableBoolean("/SmartDashboard/Tunables/Flywheels", false); + // Status signals + private StatusSignal flywheelOneRPS; + private StatusSignal flywheelOneSupplyCurrent; + + // Cache + private double timeEnteredTargetZone = 0; + private double lastTime; + private boolean hasBall = false; + // Constructor public Flywheels() { FlywheelOne = new TalonFX(Hardware.FLYWHEEL_ONE_ID); @@ -51,16 +70,20 @@ public Flywheels() { configureMotors(); var nt = NetworkTableInstance.getDefault(); + ballPub = nt.getIntegerTopic("flywheels/ballsShot").publish(); velocityTopic = nt.getDoubleTopic("/launcher/velocity"); currentTopic = nt.getDoubleTopic("/launcher/current"); velocityPub = velocityTopic.publish(); currentPub = currentTopic.publish(); velocityPub.set(0.0); currentPub.set(0.0); - + lastTime = Timer.getFPGATimestamp(); if (RobotBase.isSimulation()) { flywheelSim = new FlywheelsSim(FlywheelOne, FlywheelTwo); } + + flywheelOneRPS = FlywheelOne.getVelocity(); + flywheelOneSupplyCurrent = FlywheelOne.getSupplyCurrent(); } private void configureMotors() { @@ -152,6 +175,33 @@ public Trigger atTargetVelocityTrigger(double targetRPS, double toleranceRPS) { return new Trigger(() -> atTargetVelocity(targetRPS, toleranceRPS)); } + public boolean hasBeenAtTargetFor(double durationSeconds) { + boolean atTarget = atTargetVelocity(targetVelocity.get(), FLYWHEEL_TOLERANCE); + + // at target? + if (atTarget) { + // if no active timer + if (timeEnteredTargetZone < 0) { + // First time at target, record the timestamp. + timeEnteredTargetZone = Timer.getFPGATimestamp(); + return false; + } + } else { + // Not at target, reset the timer to -1 + timeEnteredTargetZone = -1; + } + // Check if the time at target has exceeded the duration. + boolean hasStopped = (Timer.getFPGATimestamp() - timeEnteredTargetZone) >= durationSeconds; + if (hasStopped) { + resetCachedValues(); // Reset for the next shooting sequence + } + return hasStopped; + } + + public void resetCachedValues() { + timeEnteredTargetZone = -1; + } + @Override public void simulationPeriodic() { if (flywheelSim != null) { @@ -161,9 +211,23 @@ public void simulationPeriodic() { @Override public void periodic() { + StatusSignal.refreshAll(flywheelOneSupplyCurrent, flywheelOneRPS); + double currentRPS = flywheelOneRPS.getValueAsDouble(); + double dt = Timer.getFPGATimestamp() - lastTime; + + if (dt >= 0.02) { + double slope = (currentRPS - lastRPS) / dt; + if (slope < minDerivative && !hasBall) { + hasBall = true; + } else if (slope > maxDerivative && hasBall) { + hasBall = false; + ballCount++; + } + } - velocityPub.set(FlywheelOne.getVelocity().getValueAsDouble()); - currentPub.set(FlywheelOne.getSupplyCurrent().getValueAsDouble()); + velocityPub.set(flywheelOneRPS.getValueAsDouble()); + ballPub.set(ballCount); + currentPub.set(flywheelOneSupplyCurrent.getValueAsDouble()); if (TUNER_CONTROLLED.get()) { if (targetVelocity.hasChangedSince(lastPositionUpdateTime)) { TimestampedDouble currentTarget = targetVelocity.getAtomic(); From d38c32b65be3c49de633176dffa776413c3ccbd4 Mon Sep 17 00:00:00 2001 From: Fatblabs Date: Mon, 6 Apr 2026 11:55:56 -0700 Subject: [PATCH 2/4] gemini changes --- .../robot/subsystems/launcher/Flywheels.java | 30 ++++++++----------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/Flywheels.java b/src/main/java/frc/robot/subsystems/launcher/Flywheels.java index 98939f8ec..0d182899b 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Flywheels.java +++ b/src/main/java/frc/robot/subsystems/launcher/Flywheels.java @@ -57,7 +57,7 @@ public class Flywheels extends SubsystemBase { private StatusSignal flywheelOneSupplyCurrent; // Cache - private double timeEnteredTargetZone = 0; + private double timeEnteredTargetZone = -1; private double lastTime; private boolean hasBall = false; @@ -84,6 +84,7 @@ public Flywheels() { flywheelOneRPS = FlywheelOne.getVelocity(); flywheelOneSupplyCurrent = FlywheelOne.getSupplyCurrent(); + lastRPS = flywheelOneRPS.getValueAsDouble(); } private void configureMotors() { @@ -178,24 +179,16 @@ public Trigger atTargetVelocityTrigger(double targetRPS, double toleranceRPS) { public boolean hasBeenAtTargetFor(double durationSeconds) { boolean atTarget = atTargetVelocity(targetVelocity.get(), FLYWHEEL_TOLERANCE); - // at target? - if (atTarget) { - // if no active timer - if (timeEnteredTargetZone < 0) { - // First time at target, record the timestamp. - timeEnteredTargetZone = Timer.getFPGATimestamp(); - return false; - } - } else { - // Not at target, reset the timer to -1 + if (!atTarget) { timeEnteredTargetZone = -1; + return false; } - // Check if the time at target has exceeded the duration. - boolean hasStopped = (Timer.getFPGATimestamp() - timeEnteredTargetZone) >= durationSeconds; - if (hasStopped) { - resetCachedValues(); // Reset for the next shooting sequence + + if (timeEnteredTargetZone < 0) { + timeEnteredTargetZone = Timer.getFPGATimestamp(); } - return hasStopped; + + return (Timer.getFPGATimestamp() - timeEnteredTargetZone) >= durationSeconds; } public void resetCachedValues() { @@ -213,7 +206,8 @@ public void simulationPeriodic() { public void periodic() { StatusSignal.refreshAll(flywheelOneSupplyCurrent, flywheelOneRPS); double currentRPS = flywheelOneRPS.getValueAsDouble(); - double dt = Timer.getFPGATimestamp() - lastTime; + double now = Timer.getFPGATimestamp(); + double dt = now - lastTime; if (dt >= 0.02) { double slope = (currentRPS - lastRPS) / dt; @@ -223,6 +217,8 @@ public void periodic() { hasBall = false; ballCount++; } + lastRPS = currentRPS; + lastTime = now; } velocityPub.set(flywheelOneRPS.getValueAsDouble()); From 60891541a0b7a43765197b2266ee0de19b152c68 Mon Sep 17 00:00:00 2001 From: Fatblabs Date: Sun, 19 Apr 2026 11:02:08 -0700 Subject: [PATCH 3/4] better ball counter and fixed isOutOfFuel() --- .../robot/subsystems/launcher/Flywheels.java | 36 +++++++++++++++---- 1 file changed, 29 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/Flywheels.java b/src/main/java/frc/robot/subsystems/launcher/Flywheels.java index 7b5e8a501..34c9badfa 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Flywheels.java +++ b/src/main/java/frc/robot/subsystems/launcher/Flywheels.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.launcher; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; + import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -13,6 +15,7 @@ import edu.wpi.first.networktables.IntegerPublisher; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.TimestampedDouble; +import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj.DataLogManager; @@ -35,7 +38,6 @@ public class Flywheels extends SubsystemBase { // Debounce stuff private static final double DURATION = 1; // second - private final Debouncer m_dippedDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kFalling); private final Debouncer m_recoveredDebouncer = new Debouncer(DURATION, Debouncer.DebounceType.kRising); private boolean hasDipped = false; @@ -47,13 +49,15 @@ public class Flywheels extends SubsystemBase { private VelocityTorqueCurrentFOC request = new VelocityTorqueCurrentFOC(0); public NtTunableDouble targetVelocity; - private double lastRPS = 0; private long lastPositionUpdateTime = 0; - private final double minDerivative = -2; - private final double maxDerivative = 2; + // Ball counter + private final double minAccel = -100; private IntegerPublisher ballPub; + private boolean inDip = false; private int ballCount = 0; + private boolean isShooting = false; + public final double FLYWHEEL_TOLERANCE = 15; // RPS // increased on drive practice 3/18 from 5 -> 10 //Increased to 15 by TD 3/18 public final NtTunableBoolean TUNER_CONTROLLED = @@ -62,6 +66,7 @@ public class Flywheels extends SubsystemBase { // Status signals private StatusSignal flywheelOneRPS; private StatusSignal flywheelOneSupplyCurrent; + private StatusSignal flywheelOneAccel; // Constructor public Flywheels() { @@ -82,6 +87,7 @@ public Flywheels() { flywheelOneRPS = flywheelOne.getVelocity(); flywheelOneSupplyCurrent = flywheelOne.getSupplyCurrent(); + flywheelOneAccel = flywheelOne.getAcceleration(); flywheelOne.clearStickyFaults(); flywheelTwo.clearStickyFaults(); @@ -142,6 +148,7 @@ private void applyConfig(TalonFX motor, TalonFXConfiguration config) { public Command setVelocityCommand(double rps) { return runEnd( () -> { + isShooting = true; setVelocityRPS(rps); }, () -> { @@ -152,6 +159,7 @@ public Command setVelocityCommand(double rps) { } public void setVelocityRPS(double rps) { + isShooting = true; request.Velocity = rps; flywheelOne.setControl(request); flywheelTwo.setControl(request); @@ -160,6 +168,7 @@ public void setVelocityRPS(double rps) { public Command stopCommand() { return runOnce( () -> { + isShooting = false; flywheelOne.stopMotor(); flywheelTwo.stopMotor(); }) @@ -167,6 +176,7 @@ public Command stopCommand() { } public void stop() { + isShooting = false; flywheelOne.stopMotor(); flywheelTwo.stopMotor(); } @@ -192,8 +202,7 @@ public void resetFuelCheck() { public boolean isOutOfFuel() { boolean atTarget = atTargetVelocity(request.Velocity, FLYWHEEL_TOLERANCE); - boolean stillAtTarget = m_dippedDebouncer.calculate(atTarget); - if (!stillAtTarget) { + if (!atTarget && !hasDipped) { hasDipped = true; } @@ -204,7 +213,20 @@ public boolean isOutOfFuel() { @Override public void periodic() { - StatusSignal.refreshAll(flywheelOneRPS, flywheelOneSupplyCurrent); + StatusSignal.refreshAll(flywheelOneRPS, flywheelOneSupplyCurrent, flywheelOneAccel); + + if (isShooting) { + boolean atTarget = atTargetVelocity(request.Velocity, FLYWHEEL_TOLERANCE); + double currentAccel = flywheelOneAccel.getValue().in(RotationsPerSecondPerSecond); + if (currentAccel <= minAccel && !atTarget && !inDip) { + ballCount++; + inDip = true; + ballPub.set(ballCount); + } else if (atTarget && inDip) { + inDip = false; + } + } + velocityPub.set(flywheelOneRPS.getValueAsDouble()); currentPub.set(flywheelOneSupplyCurrent.getValueAsDouble()); if (TUNER_CONTROLLED.get()) { From d0ec20643e6ada2b7bd0ae6b1c16bf086d27fe4c Mon Sep 17 00:00:00 2001 From: FrameworkDS <194180343+RobototesProgrammers@users.noreply.github.com> Date: Mon, 20 Apr 2026 16:58:32 -0700 Subject: [PATCH 4/4] better --- .../java/frc/robot/subsystems/launcher/Flywheels.java | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/launcher/Flywheels.java b/src/main/java/frc/robot/subsystems/launcher/Flywheels.java index 34c9badfa..d431f6a04 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Flywheels.java +++ b/src/main/java/frc/robot/subsystems/launcher/Flywheels.java @@ -52,7 +52,7 @@ public class Flywheels extends SubsystemBase { private long lastPositionUpdateTime = 0; // Ball counter - private final double minAccel = -100; + private final double minAccel = -21; private IntegerPublisher ballPub; private boolean inDip = false; private int ballCount = 0; @@ -84,6 +84,7 @@ public Flywheels() { currentPub = currentTopic.publish(); velocityPub.set(0.0); currentPub.set(0.0); + ballPub.set(0); flywheelOneRPS = flywheelOne.getVelocity(); flywheelOneSupplyCurrent = flywheelOne.getSupplyCurrent(); @@ -216,14 +217,11 @@ public void periodic() { StatusSignal.refreshAll(flywheelOneRPS, flywheelOneSupplyCurrent, flywheelOneAccel); if (isShooting) { - boolean atTarget = atTargetVelocity(request.Velocity, FLYWHEEL_TOLERANCE); double currentAccel = flywheelOneAccel.getValue().in(RotationsPerSecondPerSecond); - if (currentAccel <= minAccel && !atTarget && !inDip) { + if (currentAccel <= minAccel) { ballCount++; inDip = true; ballPub.set(ballCount); - } else if (atTarget && inDip) { - inDip = false; } }