From c66270dd5bf77b1c97190195cdf28f185dbb5c5c Mon Sep 17 00:00:00 2001 From: Ben Date: Mon, 7 Jul 2025 18:18:25 -0400 Subject: [PATCH 01/10] Update gradle.yml to autobuild iri branch --- .github/workflows/gradle.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml index 6e0cfce5..e96a1b81 100644 --- a/.github/workflows/gradle.yml +++ b/.github/workflows/gradle.yml @@ -6,9 +6,9 @@ name: CI # events but only for the main branch. on: push: - branches: [ main, Ferndale, District-2, States, Worlds ] + branches: [ main, Ferndale, District-2, States, Worlds, iri ] pull_request: - branches: [ main, Ferndale, District-2, States, Worlds ] + branches: [ main, Ferndale, District-2, States, Worlds, iri ] # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: From 486c60094fb00a9f7c9661d15a0337bd1f3bd39b Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Thu, 10 Jul 2025 20:23:31 -0400 Subject: [PATCH 02/10] Remove processor button --- .../src/main/java/frc/robot/binding/DriveBindings.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java index 7b49fd12..1799ff09 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/DriveBindings.java @@ -35,8 +35,8 @@ public void bind(Superstructure superstructure) { superstructure.setDrive(superstructure.enterWithoutProxy(new TeleopDrive(m_x, m_y, m_rot))); m_resetHeading.onTrue(superstructure.enter(new HeadingReset())); - m_smartAlign.and(superstructure.holdingAlgae()).whileTrue(superstructure.enter(new Align( - new APTarget(FieldConstants.k_processor).withEntryAngle(Rotation2d.kCW_Pi_2)))); + // m_smartAlign.and(superstructure.holdingAlgae()).whileTrue(superstructure.enter(new Align( + // new APTarget(FieldConstants.k_processor).withEntryAngle(Rotation2d.kCW_Pi_2)))); m_smartAlign.and(superstructure.holdingAlgae().negate()).whileTrue(superstructure.enter( new DeferredAlign(AlignLocation.Center))); m_leftAlign.whileTrue(superstructure.enter(new DeferredAlign(AlignLocation.Left))); From 7a1a93c9ed81fd62bc228fdd90b0713f7a3eac36 Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Thu, 10 Jul 2025 20:23:46 -0400 Subject: [PATCH 03/10] Move pivot home forwards a bit --- .../src/main/java/frc/robot/subsystems/pivot/PivotState.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotState.java b/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotState.java index cad2f8dd..dbde798e 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotState.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/pivot/PivotState.java @@ -12,7 +12,7 @@ public enum PivotState { /** The angle to score at the net */ Net(0.342), /** The "home" angle for the pivot */ - Stow(0.343), + Stow(0.326), /** The angle for lollipop intake */ HighGround(0.15); From 50e79f585128c0a1182d0ad1391b2857ee5faaa2 Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Thu, 10 Jul 2025 20:24:03 -0400 Subject: [PATCH 04/10] Decrease elevator unsafe height to include upper algae intake --- .../src/main/java/frc/robot/subsystems/elevator/Elevator.java | 4 ++-- .../java/frc/robot/subsystems/elevator/ElevatorConstants.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 1c0e2615..9d983261 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -110,8 +110,8 @@ public void periodic() { * Whether or not the elevator is above the "safe" range */ public Trigger unsafe() { - return new Trigger(() -> m_inputs.position > ElevatorConstants.kUnsafeRange - || m_reference.position() > ElevatorConstants.kUnsafeRange); + return new Trigger(() -> m_inputs.position >= ElevatorConstants.kUnsafeRange + || m_reference.position() >= ElevatorConstants.kUnsafeRange); } protected void passive() { diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index b258799d..d2ee40e2 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -50,7 +50,7 @@ public final class ElevatorConstants { protected static final double kZeroVoltage = -4.0; - protected static final double kUnsafeRange = ElevatorState.L2.position() + 2 * kInch; + protected static final double kUnsafeRange = ElevatorState.UpperReef.position(); protected static final double kTolerance = 0.06; From 6c3823dc2104a391dbd4771a2be0254cecd1dd5e Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Fri, 11 Jul 2025 13:46:21 -0400 Subject: [PATCH 05/10] Move starting point towards wall --- ThriftyTest/simgui.json | 13 +++++++++++ .../src/main/java/frc/robot/Constants.java | 6 +++-- .../frc/robot/binding/DashboardBindings.java | 2 ++ .../drivetrain/CommandSwerveDrivetrain.java | 5 ++++ .../superstructure/states/SeedAngle.java | 23 +++++++++++++++++++ .../main/java/frc/robot/utils/FieldUtils.java | 11 ++++++++- 6 files changed, 57 insertions(+), 3 deletions(-) create mode 100644 ThriftyTest/src/main/java/frc/robot/superstructure/states/SeedAngle.java diff --git a/ThriftyTest/simgui.json b/ThriftyTest/simgui.json index 041fd9fa..c7dcf2a3 100644 --- a/ThriftyTest/simgui.json +++ b/ThriftyTest/simgui.json @@ -54,6 +54,16 @@ "visible": true } }, + "/SmartDashboard/Prep/Set Left": { + "window": { + "visible": true + } + }, + "/SmartDashboard/Prep/Set Right": { + "window": { + "visible": true + } + }, "/SmartDashboard/Super Field": { "Algae": { "image": "../resources/algae.jpg", @@ -225,6 +235,9 @@ } } }, + "NetworkTables Info": { + "visible": true + }, "Plot": { "Plot <0>": { "plots": [ diff --git a/ThriftyTest/src/main/java/frc/robot/Constants.java b/ThriftyTest/src/main/java/frc/robot/Constants.java index 8f3fe9c8..b147a5f5 100644 --- a/ThriftyTest/src/main/java/frc/robot/Constants.java +++ b/ThriftyTest/src/main/java/frc/robot/Constants.java @@ -44,8 +44,10 @@ public static class FieldConstants { public static final Pose2d kBargeFromLeft = new Pose2d(7.5, 6.6, Rotation2d.fromDegrees(21)); public static final Pose2d kStartCenter = new Pose2d(7.076, 3.991, Rotation2d.kPi); - public static final Pose2d kStartRight = new Pose2d(7.076, 1.883, Rotation2d.kPi); - public static final Pose2d kStartLeft = new Pose2d(7.076, 6.027, Rotation2d.kPi); + public static final Pose2d kStartRight = new Pose2d(7.076, 1.2, Rotation2d.kPi); + public static final Pose2d kStartLeft = new Pose2d(7.076, 6.6, Rotation2d.kPi); + + public static final Rotation2d kStartHeading = Rotation2d.kPi; // facing our alliance } public static final class StateSpaceConstants { diff --git a/ThriftyTest/src/main/java/frc/robot/binding/DashboardBindings.java b/ThriftyTest/src/main/java/frc/robot/binding/DashboardBindings.java index 3dd81ef3..bed9c7aa 100644 --- a/ThriftyTest/src/main/java/frc/robot/binding/DashboardBindings.java +++ b/ThriftyTest/src/main/java/frc/robot/binding/DashboardBindings.java @@ -6,6 +6,7 @@ import frc.robot.superstructure.EnterableState; import frc.robot.superstructure.Superstructure; import frc.robot.superstructure.Superstructure.Subsystems; +import frc.robot.superstructure.states.SeedAngle; import frc.robot.superstructure.states.SeedPose; import frc.robot.superstructure.states.Stow; import frc.robot.superstructure.states.TrackAlgae; @@ -15,6 +16,7 @@ public void bind(Superstructure superstructure) { SmartDashboard.putData("Prep/Set Center", superstructure.enter(SeedPose.center())); SmartDashboard.putData("Prep/Set Left", superstructure.enter(SeedPose.left())); SmartDashboard.putData("Prep/Set Right", superstructure.enter(SeedPose.right())); + SmartDashboard.putData("Prep/Set Angle", superstructure.enter(SeedAngle.reverse())); SmartDashboard.putData("Test/Stow", superstructure.enter(new Stow())); SmartDashboard.putData("Test/Elevator Up", superstructure.enter(new EnterableState() { diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java index 6ddd920b..3eda403f 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/drivetrain/CommandSwerveDrivetrain.java @@ -25,6 +25,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.util.DriveFeedforwards; +import com.pathplanner.lib.util.FlippingUtil; import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.swerve.SwerveSetpoint; import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; @@ -219,6 +220,10 @@ private void setPose(Pose2d pose) { resetPose(pose); } + public Command setLocalHeading(Rotation2d heading) { + return Commands.runOnce(() -> resetRotation(FieldUtils.getLocalRotation(heading))).ignoringDisable(true); + } + private ChassisSpeeds getRobotRelativeSpeeds() { return getState().Speeds; } diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/SeedAngle.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/SeedAngle.java new file mode 100644 index 00000000..4477fbdd --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/SeedAngle.java @@ -0,0 +1,23 @@ +package frc.robot.superstructure.states; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.FieldConstants; +import frc.robot.superstructure.EnterableState; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class SeedAngle implements EnterableState { + private final Rotation2d m_angle; + + public SeedAngle(Rotation2d angle) { + m_angle = angle; + } + + public static SeedAngle reverse() { + return new SeedAngle(FieldConstants.kStartHeading); + } + + public Command build(Subsystems subsystems) { + return subsystems.drivetrain().setLocalHeading(m_angle); + } +} diff --git a/ThriftyTest/src/main/java/frc/robot/utils/FieldUtils.java b/ThriftyTest/src/main/java/frc/robot/utils/FieldUtils.java index ed8545b3..4780c3ae 100644 --- a/ThriftyTest/src/main/java/frc/robot/utils/FieldUtils.java +++ b/ThriftyTest/src/main/java/frc/robot/utils/FieldUtils.java @@ -1,11 +1,12 @@ package frc.robot.utils; import com.pathplanner.lib.util.FlippingUtil; +import com.therekrab.autopilot.APTarget; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import com.therekrab.autopilot.APTarget; public class FieldUtils { /** @@ -23,6 +24,14 @@ public static Pose2d getLocalPose(Pose2d localPose) { } } + public static Rotation2d getLocalRotation(Rotation2d rotation) { + if (DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Red)) { + return FlippingUtil.flipFieldRotation(rotation); + } else { + return rotation; + } + } + /** * Flips the target if the driver station reports being on the red alliance */ From 2e40770da441cf4205cb15a0c3d1683efacd4709 Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Fri, 11 Jul 2025 14:49:13 -0400 Subject: [PATCH 06/10] high algae not low algae --- ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto b/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto index 1b22aa02..273f4bd7 100644 --- a/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto +++ b/ThriftyTest/src/main/deploy/pathplanner/autos/Center.auto @@ -29,7 +29,7 @@ { "type": "named", "data": { - "name": "Upper Algae" + "name": "Lower Algae" } } ] From c0992fbce23411a2f564816fdf41d67719d8e95e Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Fri, 11 Jul 2025 17:20:24 -0400 Subject: [PATCH 07/10] explicit stator limit, more logging for algae --- .../java/frc/robot/subsystems/algae/AlgaeConstants.java | 7 +++++-- .../src/main/java/frc/robot/subsystems/algae/AlgaeIO.java | 1 + .../java/frc/robot/subsystems/algae/AlgaeIOHardware.java | 6 +++++- .../frc/robot/subsystems/algae/AlgaeIOInputsLogger.java | 2 ++ .../java/frc/robot/subsystems/elevator/ElevatorState.java | 4 ++-- 5 files changed, 15 insertions(+), 5 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java index 811dafdc..b0d507bc 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java @@ -12,10 +12,11 @@ public final class AlgaeConstants { protected static final double kIntakeVoltage = 12; protected static final double kNetEjectVoltage = -3.0; protected static final double kProcessorEjectVoltage = -3.2; - protected static final double kHoldVoltage = 2.7; + protected static final double kHoldVoltage = 6.0; protected static final double kTorqueCurrentThreshold = 75; protected static final double kSupplyCurrentLimit = 40.0; + protected static final double kStatorCurrentLimit = 120.0; protected static final double kProcessorScoreTime = 2.0; protected static final double kNetScoreTime = 0.4; @@ -27,6 +28,8 @@ public final class AlgaeConstants { .withCurrentLimits(new CurrentLimitsConfigs() .withSupplyCurrentLimitEnable(true) - .withSupplyCurrentLimit(kSupplyCurrentLimit)); + .withSupplyCurrentLimit(kSupplyCurrentLimit) + .withStatorCurrentLimit(kStatorCurrentLimit) + .withStatorCurrentLimitEnable(true)); } diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java index 7dcac00b..7e9f802b 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIO.java @@ -9,6 +9,7 @@ class AlgaeIOInputs { public double torque = 0.0; public double voltage = 0.0; public double temperature = 0.0; + public double stator = 0.0; } void setVoltage(double voltage); diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOHardware.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOHardware.java index 913d602c..97fe56cb 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOHardware.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOHardware.java @@ -19,6 +19,7 @@ public class AlgaeIOHardware implements AlgaeIO { private final StatusSignal m_torqueSignal; private final StatusSignal m_tempSignal; private final StatusSignal m_velocitySignal; + private final StatusSignal m_statorSignal; public AlgaeIOHardware() { m_motor = new TalonFX(AlgaeConstants.kMotorID); @@ -30,13 +31,15 @@ public AlgaeIOHardware() { m_torqueSignal = m_motor.getTorqueCurrent(); m_tempSignal = m_motor.getDeviceTemp(); m_velocitySignal = m_motor.getVelocity(); + m_statorSignal = m_motor.getStatorCurrent(); StatusSignalUtil.registerRioSignals( m_voltageSignal, m_currentSignal, m_torqueSignal, m_tempSignal, - m_velocitySignal); + m_velocitySignal, + m_statorSignal); } public void updateInputs(AlgaeIOInputs inputs) { @@ -50,6 +53,7 @@ public void updateInputs(AlgaeIOInputs inputs) { inputs.current = m_currentSignal.getValueAsDouble(); inputs.torque = m_torqueSignal.getValueAsDouble(); inputs.temperature = m_tempSignal.getValueAsDouble(); + inputs.stator = m_statorSignal.getValueAsDouble(); } public void setVoltage(double voltage) { diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOInputsLogger.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOInputsLogger.java index ca9c77ba..b04993c2 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOInputsLogger.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOInputsLogger.java @@ -12,6 +12,8 @@ public AlgaeIOInputsLogger(AlgaeIOInputs inputs) { log.registerDouble("Current", () -> inputs.current); log.registerDouble("Torque", () -> inputs.torque); log.registerDouble("Voltage", () -> inputs.voltage); + log.registerDouble("Tempterature", () -> inputs.temperature); + log.registerDouble("Stator Current", () -> inputs.stator); } public void log() { diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java index 1df67e79..2e03c724 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java @@ -18,9 +18,9 @@ public enum ElevatorState { /** Secondary L1 height for when a coral is already present */ SecondaryL1(ElevatorState.L1.position() + 8 * ElevatorConstants.kInch), /** L2 height */ - L2(4.016 + 3 * ElevatorConstants.kInch), + L2(4.016 + 4 * ElevatorConstants.kInch), /** L3 height */ - L3(7.257 - 4 * ElevatorConstants.kInch), + L3(7.257 - 5 * ElevatorConstants.kInch), /** L4 height */ L4(9.757 + 0.3 * ElevatorConstants.kInch), /** Height to score net */ From f85a59bd98e73f97f3686cc5eec42e19047ece8c Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Fri, 11 Jul 2025 20:00:54 -0400 Subject: [PATCH 08/10] udpates ig --- .../main/java/frc/robot/subsystems/algae/AlgaeConstants.java | 2 +- .../java/frc/robot/subsystems/climber/ClimberConstants.java | 2 +- .../main/java/frc/robot/subsystems/coral/CoralConstants.java | 2 +- .../java/frc/robot/subsystems/elevator/ElevatorState.java | 4 ++-- .../src/main/java/frc/robot/superstructure/states/Net.java | 4 +++- 5 files changed, 8 insertions(+), 6 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java index b0d507bc..8b4eab61 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java @@ -16,7 +16,7 @@ public final class AlgaeConstants { protected static final double kTorqueCurrentThreshold = 75; protected static final double kSupplyCurrentLimit = 40.0; - protected static final double kStatorCurrentLimit = 120.0; + protected static final double kStatorCurrentLimit = 125.0; protected static final double kProcessorScoreTime = 2.0; protected static final double kNetScoreTime = 0.4; diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java index 6605e4c1..96577895 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java @@ -25,7 +25,7 @@ public final class ClimberConstants { protected static final double kStowPosition = -0.25; protected static final double kClimbPosition = -0.110; - protected static final double kClimbReadyTolerance = -0.001; + protected static final double kClimbReadyTolerance = -0.004; protected static final double kFunnelOpenTime = 1.5; diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralConstants.java index d13ec950..1efcd4c0 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/coral/CoralConstants.java @@ -29,7 +29,7 @@ public class CoralConstants { protected static final double kL1LeftEjectVoltage = 2; protected static final double kL1RightEjectVoltage = 4; - protected static final double kSupplyCurrentLimit = 20.0; + protected static final double kSupplyCurrentLimit = 25.0; protected static final InvertedValue kInvertRight = InvertedValue.Clockwise_Positive; diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java index 2e03c724..db640942 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/elevator/ElevatorState.java @@ -20,7 +20,7 @@ public enum ElevatorState { /** L2 height */ L2(4.016 + 4 * ElevatorConstants.kInch), /** L3 height */ - L3(7.257 - 5 * ElevatorConstants.kInch), + L3(7.257 - 4 * ElevatorConstants.kInch), /** L4 height */ L4(9.757 + 0.3 * ElevatorConstants.kInch), /** Height to score net */ @@ -28,7 +28,7 @@ public enum ElevatorState { /** Height to intake algae from lower reef */ LowerReef(2.0), /** Height to intake algae from upper reef */ - UpperReef(4.5); + UpperReef(4.5 - 1.5 * ElevatorConstants.kInch); private final double m_position; diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java index b0130924..a8aeafbf 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.RobotObserver; import frc.robot.subsystems.elevator.ElevatorState; import frc.robot.subsystems.pivot.PivotState; import frc.robot.superstructure.EnterableState; @@ -23,6 +24,7 @@ public Command build(Subsystems subsystems) { .finallyDo(subsystems.elevator()::release) .finallyDo(subsystems.pivot()::release) .finallyDo(subsystems.algae()::release) - .onlyIf(subsystems.algae().holdingAlgae()); + .onlyIf(subsystems.algae().holdingAlgae()) + .unless(RobotObserver::getNoElevatorZone); } } From 31d137fdd4ff54a96bdeb8179e02bcb892d885c1 Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Sat, 12 Jul 2025 11:27:34 -0400 Subject: [PATCH 09/10] Add super debouncer to aviod dropping algae. --- .../frc/robot/subsystems/algae/Algae.java | 22 ++- .../subsystems/algae/AlgaeConstants.java | 8 +- .../subsystems/algae/AlgaeIOInputsLogger.java | 2 +- .../frc/robot/superstructure/states/Net.java | 4 +- .../java/frc/robot/utils/SuperDebouncer.java | 131 ++++++++++++++++++ 5 files changed, 161 insertions(+), 6 deletions(-) create mode 100644 ThriftyTest/src/main/java/frc/robot/utils/SuperDebouncer.java diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java index fdc9a1bc..b0ea3e87 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java @@ -1,8 +1,11 @@ package frc.robot.subsystems.algae; +import static edu.wpi.first.units.Units.Seconds; + import org.slf4j.Logger; import org.slf4j.LoggerFactory; +import frc.robot.utils.SuperDebouncer.DebounceType; import edu.wpi.first.math.filter.MedianFilter; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -12,11 +15,15 @@ import frc.robot.subsystems.PassiveSubsystem; import frc.robot.subsystems.algae.AlgaeIO.AlgaeIOInputs; import frc.robot.utils.LoopTimer; +import frc.robot.utils.OnboardLogger; +import frc.robot.utils.SuperDebouncer; public class Algae extends PassiveSubsystem { @SuppressWarnings("unused") private final Logger m_logger = LoggerFactory.getLogger(Algae.class); + private final OnboardLogger m_ologger; + private final LoopTimer m_timer; private final AlgaeIO m_io; @@ -24,6 +31,7 @@ public class Algae extends PassiveSubsystem { private AlgaeIOInputsLogger m_inputsLogger; private boolean m_hasAlgae; + private SuperDebouncer m_debouncer = new SuperDebouncer(AlgaeConstants.kAlgaeDebounceTime.in(Seconds), DebounceType.kFalling); private MedianFilter m_filter = new MedianFilter(10); @@ -34,8 +42,12 @@ public Algae() { } else { m_io = new AlgaeIOSim(); } + resetDebouncer(); m_inputs = new AlgaeIOInputs(); m_inputsLogger = new AlgaeIOInputsLogger(m_inputs); + m_ologger = new OnboardLogger("Algae"); + m_ologger.registerBoolean("Holding", holdingAlgae()); + RobotObserver.setAlgaePieceHeldSupplier(this.holdingAlgae()); m_timer = new LoopTimer("Algae"); } @@ -74,7 +86,8 @@ public void periodic() { m_timer.reset(); m_io.updateInputs(m_inputs); m_inputsLogger.log(); - m_hasAlgae = getTorqueCurrent() >= AlgaeConstants.kTorqueCurrentThreshold; + m_hasAlgae = m_debouncer.calculate(getTorqueCurrent() >= AlgaeConstants.kTorqueCurrentThreshold); + m_ologger.log(); m_timer.log(); } @@ -94,6 +107,10 @@ public Command intake() { .unless(holdingAlgae()); } + private void resetDebouncer() { + m_debouncer.overrideTimer(); + } + /** * Ejects an algae with the correct conditions for a net score */ @@ -103,6 +120,9 @@ public Command net() { Commands.waitSeconds(AlgaeConstants.kNetScoreTime)) .finallyDo(this::keep) + .finallyDo((interrupted) -> { + resetDebouncer(); + }) .onlyIf(holdingAlgae()); } diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java index 8b4eab61..7189b9f7 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeConstants.java @@ -1,11 +1,15 @@ package frc.robot.subsystems.algae; +import static edu.wpi.first.units.Units.Seconds; + import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.measure.Time; + public final class AlgaeConstants { protected static final int kMotorID = 60; @@ -14,7 +18,9 @@ public final class AlgaeConstants { protected static final double kProcessorEjectVoltage = -3.2; protected static final double kHoldVoltage = 6.0; - protected static final double kTorqueCurrentThreshold = 75; + protected static final double kTorqueCurrentThreshold = 75; // We should consider 40-55 range as well. + protected static final Time kAlgaeDebounceTime = Seconds.of(0.8); + protected static final double kSupplyCurrentLimit = 40.0; protected static final double kStatorCurrentLimit = 125.0; diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOInputsLogger.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOInputsLogger.java index b04993c2..50bcba3a 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOInputsLogger.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/AlgaeIOInputsLogger.java @@ -12,7 +12,7 @@ public AlgaeIOInputsLogger(AlgaeIOInputs inputs) { log.registerDouble("Current", () -> inputs.current); log.registerDouble("Torque", () -> inputs.torque); log.registerDouble("Voltage", () -> inputs.voltage); - log.registerDouble("Tempterature", () -> inputs.temperature); + log.registerDouble("Temperature", () -> inputs.temperature); log.registerDouble("Stator Current", () -> inputs.stator); } diff --git a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java index a8aeafbf..b0130924 100644 --- a/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java +++ b/ThriftyTest/src/main/java/frc/robot/superstructure/states/Net.java @@ -2,7 +2,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.RobotObserver; import frc.robot.subsystems.elevator.ElevatorState; import frc.robot.subsystems.pivot.PivotState; import frc.robot.superstructure.EnterableState; @@ -24,7 +23,6 @@ public Command build(Subsystems subsystems) { .finallyDo(subsystems.elevator()::release) .finallyDo(subsystems.pivot()::release) .finallyDo(subsystems.algae()::release) - .onlyIf(subsystems.algae().holdingAlgae()) - .unless(RobotObserver::getNoElevatorZone); + .onlyIf(subsystems.algae().holdingAlgae()); } } diff --git a/ThriftyTest/src/main/java/frc/robot/utils/SuperDebouncer.java b/ThriftyTest/src/main/java/frc/robot/utils/SuperDebouncer.java new file mode 100644 index 00000000..8957418c --- /dev/null +++ b/ThriftyTest/src/main/java/frc/robot/utils/SuperDebouncer.java @@ -0,0 +1,131 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.utils; + +import edu.wpi.first.math.MathSharedStore; + +/** + * A simple debounce filter for boolean streams. Requires that the boolean change value from + * baseline for a specified period of time before the filtered value changes. + */ +public class SuperDebouncer { + /** Type of debouncing to perform. */ + public enum DebounceType { + /** Rising edge. */ + kRising, + /** Falling edge. */ + kFalling, + /** Both rising and falling edges. */ + kBoth + } + + private double m_debounceTimeSeconds; + private DebounceType m_debounceType; + private boolean m_baseline; + + private double m_prevTimeSeconds; + + /** + * Creates a new Debouncer. + * + * @param debounceTime The number of seconds the value must change from baseline for the filtered + * value to change. + * @param type Which type of state change the debouncing will be performed on. + */ + public SuperDebouncer(double debounceTime, DebounceType type) { + m_debounceTimeSeconds = debounceTime; + m_debounceType = type; + + resetTimer(); + + m_baseline = + switch (m_debounceType) { + case kBoth, kRising -> false; + case kFalling -> true; + }; + } + + /** + * Creates a new Debouncer. Baseline value defaulted to "false." + * + * @param debounceTime The number of seconds the value must change from baseline for the filtered + * value to change. + */ + public SuperDebouncer(double debounceTime) { + this(debounceTime, DebounceType.kRising); + } + + private void resetTimer() { + m_prevTimeSeconds = MathSharedStore.getTimestamp(); + } + + private boolean hasElapsed() { + return MathSharedStore.getTimestamp() - m_prevTimeSeconds >= m_debounceTimeSeconds; + } + + public void overrideTimer() { + m_prevTimeSeconds = 0; + } + + /** + * Applies the debouncer to the input stream. + * + * @param input The current value of the input stream. + * @return The debounced value of the input stream. + */ + public boolean calculate(boolean input) { + if (input == m_baseline) { + resetTimer(); + } + + if (hasElapsed()) { + if (m_debounceType == DebounceType.kBoth) { + m_baseline = input; + resetTimer(); + } + return input; + } else { + return m_baseline; + } + } + + /** + * Sets the time to debounce. + * + * @param time The number of seconds the value must change from baseline for the filtered value to + * change. + */ + public void setDebounceTime(double time) { + m_debounceTimeSeconds = time; + } + + /** + * Gets the time to debounce. + * + * @return The number of seconds the value must change from baseline for the filtered value to + * change. + */ + public double getDebounceTime() { + return m_debounceTimeSeconds; + } + + /** + * Sets the debounce type. + * + * @param type Which type of state change the debouncing will be performed on. + */ + public void setDebounceType(DebounceType type) { + m_debounceType = type; + } + + /** + * Gets the debounce type. + * + * @return Which type of state change the debouncing will be performed on. + */ + public DebounceType getDebounceType() { + return m_debounceType; + } +} From 86829e00c1f345ef705726e2f42d5a2a013eb6a5 Mon Sep 17 00:00:00 2001 From: Comp Laptop Date: Sat, 12 Jul 2025 12:07:28 -0400 Subject: [PATCH 10/10] Consistency is key --- .../src/main/java/frc/robot/subsystems/algae/Algae.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java index b0ea3e87..70cb2eb0 100644 --- a/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java +++ b/ThriftyTest/src/main/java/frc/robot/subsystems/algae/Algae.java @@ -120,9 +120,7 @@ public Command net() { Commands.waitSeconds(AlgaeConstants.kNetScoreTime)) .finallyDo(this::keep) - .finallyDo((interrupted) -> { - resetDebouncer(); - }) + .finallyDo(this::resetDebouncer) .onlyIf(holdingAlgae()); }