From 6608db47c000cd3e1c085da1dad3df100bc8242d Mon Sep 17 00:00:00 2001 From: James Guleno Date: Fri, 31 Jan 2025 19:25:52 -0800 Subject: [PATCH 01/11] add climber --- .../org/team5924/frc2025/BuildConstants.java | 12 +- .../java/org/team5924/frc2025/Constants.java | 12 ++ .../org/team5924/frc2025/RobotContainer.java | 7 ++ .../java/org/team5924/frc2025/RobotState.java | 8 ++ .../frc2025/subsystems/climber/Climber.java | 67 +++++++++++ .../frc2025/subsystems/climber/ClimberIO.java | 39 +++++++ .../subsystems/climber/ClimberIOTalonFX.java | 106 ++++++++++++++++++ .../climber/LoggedTunableNumber.java | 5 + .../MechanicalRuntimeException.java | 22 ++-- 9 files changed, 261 insertions(+), 17 deletions(-) create mode 100644 src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java create mode 100644 src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java create mode 100644 src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java create mode 100644 src/main/java/org/team5924/frc2025/subsystems/climber/LoggedTunableNumber.java diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index 8547620..1a70101 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,12 +21,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 9; - public static final String GIT_SHA = "d66c6ccc27a4eeddac14c357fac10f788335678d"; - public static final String GIT_DATE = "2025-01-16 23:45:57 EST"; - public static final String GIT_BRANCH = "17-fix-license-header-file"; - public static final String BUILD_DATE = "2025-01-20 16:59:20 EST"; - public static final long BUILD_UNIX_TIME = 1737410360454L; + public static final int GIT_REVISION = 12; + public static final String GIT_SHA = "94a0328493658f5d13522822a1b508ac02497aff"; + public static final String GIT_DATE = "2025-01-23 23:18:28 EST"; + public static final String GIT_BRANCH = "7-climber-basic-functionality"; + public static final String BUILD_DATE = "2025-01-31 21:29:44 EST"; + public static final long BUILD_UNIX_TIME = 1738376984344L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team5924/frc2025/Constants.java b/src/main/java/org/team5924/frc2025/Constants.java index 6873fac..2d8575b 100644 --- a/src/main/java/org/team5924/frc2025/Constants.java +++ b/src/main/java/org/team5924/frc2025/Constants.java @@ -37,4 +37,16 @@ public static enum Mode { /** Replaying from a log file. */ REPLAY } + + /* ### Subsystems ### */ + /* Climber */ + public static final double LOOP_PERIODIC_SECONDS = 0; + public static final int SHOOTER_CAN_ID = 0; + public static final String SHOOTER_BUS = "rio"; + public static final int SHOOTER_CURRENT_LIMIT = 40; + public static final boolean SHOOTER_INVERT = false; + public static final boolean SHOOTER_BRAKE = true; + public static final double SHOOTER_REDUCTION = 18.0 / 12.0; + public static final double SHOOTER_SIM_MOI = 0.001; + // TODO: replace with real values } diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index 5e7615d..52702ec 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -28,6 +28,9 @@ import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.team5924.frc2025.commands.DriveCommands; import org.team5924.frc2025.generated.TunerConstants; +import org.team5924.frc2025.subsystems.climber.Climber; +import org.team5924.frc2025.subsystems.climber.ClimberIO; +import org.team5924.frc2025.subsystems.climber.ClimberIOTalonFX; import org.team5924.frc2025.subsystems.drive.Drive; import org.team5924.frc2025.subsystems.drive.GyroIO; import org.team5924.frc2025.subsystems.drive.GyroIOPigeon2; @@ -44,6 +47,7 @@ public class RobotContainer { // Subsystems private final Drive drive; + private final Climber climber; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -63,6 +67,7 @@ public RobotContainer() { new ModuleIOTalonFX(TunerConstants.FrontRight), new ModuleIOTalonFX(TunerConstants.BackLeft), new ModuleIOTalonFX(TunerConstants.BackRight)); + climber = new Climber(new ClimberIOTalonFX()); break; case SIM: @@ -74,6 +79,7 @@ public RobotContainer() { new ModuleIOSim(TunerConstants.FrontRight), new ModuleIOSim(TunerConstants.BackLeft), new ModuleIOSim(TunerConstants.BackRight)); + climber = new Climber(new ClimberIO() {}); break; default: @@ -85,6 +91,7 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); + climber = new Climber(new ClimberIO() {}); break; } diff --git a/src/main/java/org/team5924/frc2025/RobotState.java b/src/main/java/org/team5924/frc2025/RobotState.java index cd3fae1..b7a33bb 100644 --- a/src/main/java/org/team5924/frc2025/RobotState.java +++ b/src/main/java/org/team5924/frc2025/RobotState.java @@ -18,7 +18,10 @@ import edu.wpi.first.math.geometry.Pose2d; import lombok.Getter; +import lombok.Setter; import org.littletonrobotics.junction.AutoLogOutput; +import org.team5924.frc2025.subsystems.climber.Climber; +import org.team5924.frc2025.subsystems.climber.Climber.ClimberState; public class RobotState { private static RobotState instance; @@ -36,4 +39,9 @@ public static RobotState getInstance() { @Getter @AutoLogOutput(key = "RobotState/EstimatedPose") private Pose2d estimatedPose = new Pose2d(); + + /* ### Coral In and Out ### */ + @Setter + @AutoLogOutput(key = "RobotState/IntakeState") + private ClimberState climberState = ClimberState.STOW; } diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java b/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java new file mode 100644 index 0000000..1e88d78 --- /dev/null +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java @@ -0,0 +1,67 @@ +/* + * Climber.java + */ + +/* + * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been seperated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2025.subsystems.climber; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import lombok.Getter; +import lombok.RequiredArgsConstructor; +import org.littletonrobotics.junction.Logger; +import org.team5924.frc2025.RobotState; + +public class Climber extends SubsystemBase { + @RequiredArgsConstructor + @Getter + public enum ClimberState { + CLIMB, + STOW, + READY_TO_CLIMB, + MOVING + } + + private ClimberState goalState = ClimberState.STOW; + + @Getter private final ClimberIO io; + + private final ClimberIOInputsAutoLogged inputs = new ClimberIOInputsAutoLogged(); + + public Climber(ClimberIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Climber", inputs); + } + + public void setGoalState(ClimberState goalState) { + switch (goalState) { + case CLIMB -> RobotState.getInstance().setClimberState(ClimberState.CLIMB); + case STOW -> RobotState.getInstance().setClimberState(ClimberState.STOW); + case READY_TO_CLIMB -> RobotState.getInstance().setClimberState(ClimberState.READY_TO_CLIMB); + case MOVING -> RobotState.getInstance().setClimberState(ClimberState.MOVING); + } + } + + public void runVolts(double volts) { + io.runVolts(volts); + } + + public void setAngle(double rads) { + io.setAngle(rads); + } +} diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java new file mode 100644 index 0000000..856caeb --- /dev/null +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java @@ -0,0 +1,39 @@ +/* + * ClimberIO.java + */ + +/* + * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been seperated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2025.subsystems.climber; + +import org.littletonrobotics.junction.AutoLog; + +public interface ClimberIO { + @AutoLog + public static class ClimberIOInputs { + + public boolean motorConnected = true; + public double positionRads = 0.0; + public double velocityRadsPerSec = 0.0; + public double appliedVoltage = 0.0; + public double supplyCurrentAmps = 0.0; + public double torqueCurrentAmps = 0.0; + public double tempCelsius = 0.0; + } + + public default void updateInputs(ClimberIOInputs inputs) {} + + public default void runVolts(double volts) {} + + public default void setAngle(double rads) {} +} diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java new file mode 100644 index 0000000..fc5d297 --- /dev/null +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java @@ -0,0 +1,106 @@ +/* + * ClimberIOTalonFX.java + */ + +/* + * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been seperated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2025.subsystems.climber; + +import static edu.wpi.first.units.Units.Radians; + +import org.team5924.frc2025.Constants; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; +import edu.wpi.first.units.measure.Voltage; + +/** Add your docs here. */ +public class ClimberIOTalonFX implements ClimberIO { + private final TalonFX talon; + + private final StatusSignal position; + private final StatusSignal velocity; + private final StatusSignal appliedVoltage; + private final StatusSignal supplyCurrent; + private final StatusSignal torqueCurrent; + private final StatusSignal tempCelsius; + + // Single shot for voltage mode, robot loop will call continuously + private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0); + private final PositionVoltage positionOut = + new PositionVoltage(0).withUpdateFreqHz(0.0).withEnableFOC(true); + + private final double reduction; + + public ClimberIOTalonFX() { + reduction = Constants.SHOOTER_REDUCTION; + talon = new TalonFX(Constants.SHOOTER_CAN_ID, Constants.SHOOTER_BUS); + + // Configure TalonFX + TalonFXConfiguration config = new TalonFXConfiguration(); + config.MotorOutput.Inverted = + Constants.SHOOTER_INVERT ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; + config.MotorOutput.NeutralMode = Constants.SHOOTER_BRAKE ? NeutralModeValue.Brake : NeutralModeValue.Coast; + config.CurrentLimits.SupplyCurrentLimit = Constants.SHOOTER_CURRENT_LIMIT; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + talon.getConfigurator().apply(config); + + // Get select status signals and set update frequency + position = talon.getPosition(); + velocity = talon.getVelocity(); + appliedVoltage = talon.getMotorVoltage(); + supplyCurrent = talon.getSupplyCurrent(); + torqueCurrent = talon.getTorqueCurrent(); + tempCelsius = talon.getDeviceTemp(); + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, position, velocity, appliedVoltage, supplyCurrent, torqueCurrent, tempCelsius); + + // Disables status signals not called for update above + talon.optimizeBusUtilization(0, 1.0); + } + + @Override + public void updateInputs(ClimberIOInputs inputs) { + inputs.motorConnected = + BaseStatusSignal.refreshAll( + position, velocity, appliedVoltage, supplyCurrent, torqueCurrent, tempCelsius) + .isOK(); + inputs.positionRads = Units.rotationsToRadians(position.getValueAsDouble()) / reduction; + inputs.velocityRadsPerSec = Units.rotationsToRadians(velocity.getValueAsDouble()) / reduction; + inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); + inputs.supplyCurrentAmps = supplyCurrent.getValueAsDouble(); + inputs.torqueCurrentAmps = torqueCurrent.getValueAsDouble(); + inputs.tempCelsius = tempCelsius.getValueAsDouble(); + } + + @Override + public void runVolts(double volts) { + talon.setControl(voltageOut.withOutput(volts)); + } + + @Override + public void setAngle(double rads) { + talon.setControl(positionOut.withPosition(Radians.of(rads))); + } +} diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/LoggedTunableNumber.java b/src/main/java/org/team5924/frc2025/subsystems/climber/LoggedTunableNumber.java new file mode 100644 index 0000000..d8e2d4d --- /dev/null +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/LoggedTunableNumber.java @@ -0,0 +1,5 @@ +package org.team5924.frc2025.subsystems.climber; + +public class LoggedTunableNumber { + +} diff --git a/src/main/java/org/team5924/frc2025/util/exceptions/MechanicalRuntimeException.java b/src/main/java/org/team5924/frc2025/util/exceptions/MechanicalRuntimeException.java index 74a8f5b..9654808 100644 --- a/src/main/java/org/team5924/frc2025/util/exceptions/MechanicalRuntimeException.java +++ b/src/main/java/org/team5924/frc2025/util/exceptions/MechanicalRuntimeException.java @@ -2,17 +2,17 @@ * MechanicalRuntimeException.java */ - /* - * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been seperated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ +/* + * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been seperated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ package org.team5924.frc2025.util.exceptions; From a28014bae768969c83938d4c521f36def0652414 Mon Sep 17 00:00:00 2001 From: Jack Doherty Date: Fri, 31 Jan 2025 19:42:43 -0800 Subject: [PATCH 02/11] Merge resolution, formatter --- .../org/team5924/frc2025/BuildConstants.java | 12 +++++----- .../org/team5924/frc2025/RobotContainer.java | 22 +++++++++---------- .../java/org/team5924/frc2025/RobotState.java | 8 +++---- .../frc2025/subsystems/climber/Climber.java | 2 +- .../frc2025/subsystems/climber/ClimberIO.java | 2 +- .../subsystems/climber/ClimberIOTalonFX.java | 12 +++++----- .../climber/LoggedTunableNumber.java | 20 ++++++++++++++--- 7 files changed, 46 insertions(+), 32 deletions(-) diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index cc33540..1f64a3a 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,12 +21,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 30; - public static final String GIT_SHA = "846bad396c6828653340e26accde00ba62f26ed2"; - public static final String GIT_DATE = "2025-01-31 22:06:24 EST"; - public static final String GIT_BRANCH = "hotfix-license-header-typo"; - public static final String BUILD_DATE = "2025-01-31 22:07:53 EST"; - public static final long BUILD_UNIX_TIME = 1738379273646L; + public static final int GIT_REVISION = 34; + public static final String GIT_SHA = "4b0a9df27744b3afe90c099e5ac15f7925402961"; + public static final String GIT_DATE = "2025-01-31 22:40:55 EST"; + public static final String GIT_BRANCH = "7-climber-basic-functionality"; + public static final String BUILD_DATE = "2025-01-31 22:41:51 EST"; + public static final long BUILD_UNIX_TIME = 1738381311335L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index d4df9ac..c23bc32 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -16,6 +16,15 @@ package org.team5924.frc2025; +import com.pathplanner.lib.auto.AutoBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.team5924.frc2025.commands.DriveCommands; import org.team5924.frc2025.generated.TunerConstants; @@ -33,17 +42,6 @@ import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIOKrakenFOC; import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIOSim; -import com.pathplanner.lib.auto.AutoBuilder; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; - /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -88,7 +86,7 @@ public RobotContainer() { new ModuleIOSim(TunerConstants.FrontRight), new ModuleIOSim(TunerConstants.BackLeft), new ModuleIOSim(TunerConstants.BackRight)); - climber = new Climber(new ClimberIO() {}); + climber = new Climber(new ClimberIO() {}); coralInAndOut = new CoralInAndOut(new CoralInAndOutIOSim()); break; diff --git a/src/main/java/org/team5924/frc2025/RobotState.java b/src/main/java/org/team5924/frc2025/RobotState.java index 363e461..a651bb0 100644 --- a/src/main/java/org/team5924/frc2025/RobotState.java +++ b/src/main/java/org/team5924/frc2025/RobotState.java @@ -16,13 +16,12 @@ package org.team5924.frc2025; -import org.littletonrobotics.junction.AutoLogOutput; -import org.team5924.frc2025.subsystems.climber.Climber.ClimberState; -import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState; - import edu.wpi.first.math.geometry.Pose2d; import lombok.Getter; import lombok.Setter; +import org.littletonrobotics.junction.AutoLogOutput; +import org.team5924.frc2025.subsystems.climber.Climber.ClimberState; +import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState; @Getter public class RobotState { @@ -44,6 +43,7 @@ public static RobotState getInstance() { @Setter @AutoLogOutput(key = "RobotState/IntakeState") private ClimberState climberState = ClimberState.STOW; + @Getter @Setter @AutoLogOutput(key = "RobotState/CoralState") diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java b/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java index 1e88d78..4d9ad81 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java @@ -9,7 +9,7 @@ * Public License v3.0. A copy of this license can be found in LICENSE.md * at the root of this project. * - * If this file has been seperated from the original project, you should have + * If this file has been separated from the original project, you should have * received a copy of the GNU General Public License along with it. * If you did not, see . */ diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java index 856caeb..b8a9209 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java @@ -9,7 +9,7 @@ * Public License v3.0. A copy of this license can be found in LICENSE.md * at the root of this project. * - * If this file has been seperated from the original project, you should have + * If this file has been separated from the original project, you should have * received a copy of the GNU General Public License along with it. * If you did not, see . */ diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java index fc5d297..08a3210 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java @@ -9,7 +9,7 @@ * Public License v3.0. A copy of this license can be found in LICENSE.md * at the root of this project. * - * If this file has been seperated from the original project, you should have + * If this file has been separated from the original project, you should have * received a copy of the GNU General Public License along with it. * If you did not, see . */ @@ -18,8 +18,6 @@ import static edu.wpi.first.units.Units.Radians; -import org.team5924.frc2025.Constants; - import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -34,6 +32,7 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; +import org.team5924.frc2025.Constants; /** Add your docs here. */ public class ClimberIOTalonFX implements ClimberIO { @@ -60,8 +59,11 @@ public ClimberIOTalonFX() { // Configure TalonFX TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.Inverted = - Constants.SHOOTER_INVERT ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; - config.MotorOutput.NeutralMode = Constants.SHOOTER_BRAKE ? NeutralModeValue.Brake : NeutralModeValue.Coast; + Constants.SHOOTER_INVERT + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + config.MotorOutput.NeutralMode = + Constants.SHOOTER_BRAKE ? NeutralModeValue.Brake : NeutralModeValue.Coast; config.CurrentLimits.SupplyCurrentLimit = Constants.SHOOTER_CURRENT_LIMIT; config.CurrentLimits.SupplyCurrentLimitEnable = true; talon.getConfigurator().apply(config); diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/LoggedTunableNumber.java b/src/main/java/org/team5924/frc2025/subsystems/climber/LoggedTunableNumber.java index d8e2d4d..a45209a 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/LoggedTunableNumber.java +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/LoggedTunableNumber.java @@ -1,5 +1,19 @@ -package org.team5924.frc2025.subsystems.climber; +/* + * LoggedTunableNumber.java + */ + +/* + * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ -public class LoggedTunableNumber { +package org.team5924.frc2025.subsystems.climber; -} +public class LoggedTunableNumber {} From 942123efc9cbba4ae597ef7383db5a5d7b0f82a9 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 4 Feb 2025 16:59:22 -0800 Subject: [PATCH 03/11] add climber voltage states, climber logging, ClimberIOSim; fix climber state updating --- .vscode/settings.json | 3 +- .../org/team5924/frc2025/BuildConstants.java | 10 +-- .../java/org/team5924/frc2025/Constants.java | 9 +++ .../org/team5924/frc2025/RobotContainer.java | 14 ++++- .../java/org/team5924/frc2025/RobotState.java | 5 +- .../frc2025/subsystems/climber/Climber.java | 52 ++++++++++++++-- .../frc2025/subsystems/climber/ClimberIO.java | 2 + .../subsystems/climber/ClimberIOSim.java | 61 +++++++++++++++++++ .../climber/LoggedTunableNumber.java | 19 ------ 9 files changed, 142 insertions(+), 33 deletions(-) create mode 100644 src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOSim.java delete mode 100644 src/main/java/org/team5924/frc2025/subsystems/climber/LoggedTunableNumber.java diff --git a/.vscode/settings.json b/.vscode/settings.json index 868594c..71f3b3d 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -68,5 +68,6 @@ }, "[java]": { "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" - } + }, + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" } diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index 1f64a3a..25500bc 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,12 +21,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 34; - public static final String GIT_SHA = "4b0a9df27744b3afe90c099e5ac15f7925402961"; - public static final String GIT_DATE = "2025-01-31 22:40:55 EST"; + public static final int GIT_REVISION = 35; + public static final String GIT_SHA = "a28014bae768969c83938d4c521f36def0652414"; + public static final String GIT_DATE = "2025-01-31 22:42:43 EST"; public static final String GIT_BRANCH = "7-climber-basic-functionality"; - public static final String BUILD_DATE = "2025-01-31 22:41:51 EST"; - public static final long BUILD_UNIX_TIME = 1738381311335L; + public static final String BUILD_DATE = "2025-02-04 19:54:53 EST"; + public static final long BUILD_UNIX_TIME = 1738716893876L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team5924/frc2025/Constants.java b/src/main/java/org/team5924/frc2025/Constants.java index 502b3c6..011745a 100644 --- a/src/main/java/org/team5924/frc2025/Constants.java +++ b/src/main/java/org/team5924/frc2025/Constants.java @@ -45,6 +45,15 @@ public static enum Mode { /* General */ public static final double LOOP_PERIODIC_SECONDS = 0.02; /* Climber */ + public static final int CLIMBER_CAN_ID = 0; + public static final String CLIMBER_BUS = "rio"; + public static final int CLIMBER_CURRENT_LIMIT = 40; + public static final boolean CLIMBER_INVERT = false; + public static final boolean CLIMBER_BRAKE = true; + public static final double CLIMBER_REDUCTION = 18.0 / 12.0; + public static final double CLIMBER_SIM_MOI = 0.001; + + /* SHOOTER */ public static final int SHOOTER_CAN_ID = 0; public static final String SHOOTER_BUS = "rio"; public static final int SHOOTER_CURRENT_LIMIT = 40; diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index c23bc32..8a694c4 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -30,6 +30,7 @@ import org.team5924.frc2025.generated.TunerConstants; import org.team5924.frc2025.subsystems.climber.Climber; import org.team5924.frc2025.subsystems.climber.ClimberIO; +import org.team5924.frc2025.subsystems.climber.ClimberIOSim; import org.team5924.frc2025.subsystems.climber.ClimberIOTalonFX; import org.team5924.frc2025.subsystems.drive.Drive; import org.team5924.frc2025.subsystems.drive.GyroIO; @@ -86,7 +87,7 @@ public RobotContainer() { new ModuleIOSim(TunerConstants.FrontRight), new ModuleIOSim(TunerConstants.BackLeft), new ModuleIOSim(TunerConstants.BackRight)); - climber = new Climber(new ClimberIO() {}); + climber = new Climber(new ClimberIOSim()); coralInAndOut = new CoralInAndOut(new CoralInAndOutIOSim()); break; @@ -175,6 +176,17 @@ private void configureButtonBindings() { .rightTrigger() .onTrue( Commands.runOnce(() -> coralInAndOut.setGoalState(CoralInAndOut.CoralState.LOADING))); + + // Climber + driveController + .pov(180) + .onTrue(Commands.runOnce(() -> climber.setGoalState(Climber.ClimberState.MOVING))); + + driveController + .pov(-1) + .onTrue(Commands.runOnce(() -> climber.setGoalState(Climber.ClimberState.STOW))); + + System.out.println("Is the driver controller connected? " + driveController.isConnected()); } /** diff --git a/src/main/java/org/team5924/frc2025/RobotState.java b/src/main/java/org/team5924/frc2025/RobotState.java index a651bb0..4913d9f 100644 --- a/src/main/java/org/team5924/frc2025/RobotState.java +++ b/src/main/java/org/team5924/frc2025/RobotState.java @@ -39,11 +39,12 @@ public static RobotState getInstance() { @AutoLogOutput(key = "RobotState/EstimatedPose") private Pose2d estimatedPose = new Pose2d(); - /* ### Coral In and Out ### */ + /* ### Climber ### */ @Setter - @AutoLogOutput(key = "RobotState/IntakeState") + @AutoLogOutput(key = "RobotState/ClimberState") private ClimberState climberState = ClimberState.STOW; + /* ### Coral In and Out ### */ @Getter @Setter @AutoLogOutput(key = "RobotState/CoralState") diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java b/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java index 4d9ad81..1850c7c 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java @@ -16,23 +16,45 @@ package org.team5924.frc2025.subsystems.climber; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; +import lombok.Setter; import org.littletonrobotics.junction.Logger; import org.team5924.frc2025.RobotState; +import org.team5924.frc2025.util.LoggedTunableNumber; +@Setter +@Getter public class Climber extends SubsystemBase { + + public interface VoltageState { + DoubleSupplier getVoltageSupplier(); + } + @RequiredArgsConstructor @Getter - public enum ClimberState { - CLIMB, - STOW, - READY_TO_CLIMB, - MOVING + public enum ClimberState implements VoltageState { + // Finished climbing? + CLIMB(new LoggedTunableNumber("CoralInAndOut/ClimbingVoltage", 0.0)), + // Tucked in + STOW(new LoggedTunableNumber("CoralInAndOut/StowVoltage", 0.0)), + // Ready to climb + READY_TO_CLIMB(new LoggedTunableNumber("CoralInAndOut/ReadyToClimbVoltage", 0.0)), + // Climber is moving down + MOVING(new LoggedTunableNumber("CoralInAndOut/MovingVoltage", -12.0)); + + private final DoubleSupplier voltageSupplier; } private ClimberState goalState = ClimberState.STOW; + private ClimberState lastState; + + private final Alert disconnected; + protected final Timer stateTimer = new Timer(); @Getter private final ClimberIO io; @@ -40,15 +62,35 @@ public enum ClimberState { public Climber(ClimberIO io) { this.io = io; + + disconnected = new Alert("Climber disconnected!", Alert.AlertType.kWarning); + stateTimer.start(); } @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs("Climber", inputs); + + disconnected.set(!inputs.motorConnected); + + if (getGoalState() != lastState) { + stateTimer.reset(); + lastState = getGoalState(); + System.out.println("New goal state: " + getGoalState().name()); + } + + io.runVolts(goalState.getVoltageSupplier().getAsDouble()); + Logger.recordOutput("Climber/Climber Goal", goalState.toString()); } + /** + * Sets the goal state of the climber. + * + * @param goalState the new goal state + */ public void setGoalState(ClimberState goalState) { + this.goalState = goalState; switch (goalState) { case CLIMB -> RobotState.getInstance().setClimberState(ClimberState.CLIMB); case STOW -> RobotState.getInstance().setClimberState(ClimberState.STOW); diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java index b8a9209..77b61ef 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java @@ -36,4 +36,6 @@ public default void updateInputs(ClimberIOInputs inputs) {} public default void runVolts(double volts) {} public default void setAngle(double rads) {} + + default void stop() {} } diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOSim.java b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOSim.java new file mode 100644 index 0000000..d7b3790 --- /dev/null +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOSim.java @@ -0,0 +1,61 @@ +/* + * ClimberIOSim.java + */ + +/* + * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2025.subsystems.climber; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import org.team5924.frc2025.Constants; + +public class ClimberIOSim implements ClimberIO { + private final DCMotorSim motorSim; + private double appliedVoltage = 0.0; + private static final DCMotor CLIMBER_GEARBOX = DCMotor.getKrakenX60Foc(1); + + public ClimberIOSim() { + motorSim = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(CLIMBER_GEARBOX, Constants.CLIMBER_REDUCTION, 0.001), + CLIMBER_GEARBOX); + } + + @Override + public void updateInputs(ClimberIOInputs inputs) { + if (DriverStation.isDisabled()) { + runVolts(0.0); + } + + motorSim.update(Constants.LOOP_PERIODIC_SECONDS); + inputs.positionRads = motorSim.getAngularPositionRad(); + inputs.velocityRadsPerSec = motorSim.getAngularVelocityRadPerSec(); + inputs.appliedVoltage = appliedVoltage; + inputs.supplyCurrentAmps = motorSim.getCurrentDrawAmps(); + } + + @Override + public void runVolts(double volts) { + appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); + motorSim.setInputVoltage(appliedVoltage); + } + + @Override + public void stop() { + runVolts(0.0); + } +} diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/LoggedTunableNumber.java b/src/main/java/org/team5924/frc2025/subsystems/climber/LoggedTunableNumber.java deleted file mode 100644 index a45209a..0000000 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/LoggedTunableNumber.java +++ /dev/null @@ -1,19 +0,0 @@ -/* - * LoggedTunableNumber.java - */ - -/* - * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2025.subsystems.climber; - -public class LoggedTunableNumber {} From c956f7176da2e91028bb9def655178e3d537d01b Mon Sep 17 00:00:00 2001 From: James Guleno Date: Fri, 7 Feb 2025 19:36:23 -0800 Subject: [PATCH 04/11] resolve review fixes, add climber LaserCAN, improve climber control bindings --- .../org/team5924/frc2025/BuildConstants.java | 10 ++--- .../java/org/team5924/frc2025/Constants.java | 5 ++- .../org/team5924/frc2025/RobotContainer.java | 19 +++++++-- .../frc2025/subsystems/climber/Climber.java | 28 ++++++++++--- .../frc2025/subsystems/climber/ClimberIO.java | 6 ++- .../subsystems/climber/ClimberIOTalonFX.java | 40 +++++++++++++++++++ 6 files changed, 91 insertions(+), 17 deletions(-) diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index 25500bc..f2ee9d7 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,12 +21,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 35; - public static final String GIT_SHA = "a28014bae768969c83938d4c521f36def0652414"; - public static final String GIT_DATE = "2025-01-31 22:42:43 EST"; + public static final int GIT_REVISION = 36; + public static final String GIT_SHA = "942123efc9cbba4ae597ef7383db5a5d7b0f82a9"; + public static final String GIT_DATE = "2025-02-04 19:59:22 EST"; public static final String GIT_BRANCH = "7-climber-basic-functionality"; - public static final String BUILD_DATE = "2025-02-04 19:54:53 EST"; - public static final long BUILD_UNIX_TIME = 1738716893876L; + public static final String BUILD_DATE = "2025-02-07 22:19:33 EST"; + public static final long BUILD_UNIX_TIME = 1738984773492L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team5924/frc2025/Constants.java b/src/main/java/org/team5924/frc2025/Constants.java index 011745a..7d1aabc 100644 --- a/src/main/java/org/team5924/frc2025/Constants.java +++ b/src/main/java/org/team5924/frc2025/Constants.java @@ -45,13 +45,16 @@ public static enum Mode { /* General */ public static final double LOOP_PERIODIC_SECONDS = 0.02; /* Climber */ - public static final int CLIMBER_CAN_ID = 0; + public static final int CLIMBER_CAN_ID = 0; // TODO: update ID public static final String CLIMBER_BUS = "rio"; public static final int CLIMBER_CURRENT_LIMIT = 40; public static final boolean CLIMBER_INVERT = false; public static final boolean CLIMBER_BRAKE = true; public static final double CLIMBER_REDUCTION = 18.0 / 12.0; public static final double CLIMBER_SIM_MOI = 0.001; + public static final double CLIMBER_MIN_RADS = -Math.PI / 2; + public static final double CLIMBER_MAX_RADS = Math.PI / 2; + public static final int CLIMBER_LASER_CAN_ID = 0; // TODO: update ID /* SHOOTER */ public static final int SHOOTER_CAN_ID = 0; diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index 8a694c4..959018e 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -178,15 +178,26 @@ private void configureButtonBindings() { Commands.runOnce(() -> coralInAndOut.setGoalState(CoralInAndOut.CoralState.LOADING))); // Climber + + // Dpad Down driveController .pov(180) - .onTrue(Commands.runOnce(() -> climber.setGoalState(Climber.ClimberState.MOVING))); + .onTrue( + Commands.runOnce(() -> climber.setGoalState(Climber.ClimberState.MOVING)) + .finallyDo(() -> climber.setVoltageMultiplier(-1))); + // Dpad Up driveController - .pov(-1) - .onTrue(Commands.runOnce(() -> climber.setGoalState(Climber.ClimberState.STOW))); + .pov(0) + .onTrue( + Commands.runOnce(() -> climber.setGoalState(Climber.ClimberState.MOVING)) + .finallyDo(() -> climber.setVoltageMultiplier(1))); - System.out.println("Is the driver controller connected? " + driveController.isConnected()); + // No Dpad Up or Dpad Down + driveController + .pov(180) + .or(driveController.pov(0)) + .onFalse(Commands.runOnce(() -> climber.setGoalState(Climber.ClimberState.STOW))); } /** diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java b/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java index 1850c7c..60aa810 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java @@ -39,17 +39,23 @@ public interface VoltageState { @Getter public enum ClimberState implements VoltageState { // Finished climbing? - CLIMB(new LoggedTunableNumber("CoralInAndOut/ClimbingVoltage", 0.0)), + CLIMB(new LoggedTunableNumber("Climber/ClimbingVoltage", 0.0)), + // Tucked in - STOW(new LoggedTunableNumber("CoralInAndOut/StowVoltage", 0.0)), + STOW(new LoggedTunableNumber("Climber/StowVoltage", 0.0)), + // Ready to climb - READY_TO_CLIMB(new LoggedTunableNumber("CoralInAndOut/ReadyToClimbVoltage", 0.0)), + READY_TO_CLIMB(new LoggedTunableNumber("Climber/ReadyToClimbVoltage", 0.0)), + // Climber is moving down - MOVING(new LoggedTunableNumber("CoralInAndOut/MovingVoltage", -12.0)); + MOVING(new LoggedTunableNumber("Climber/MovingVoltage", 12.0)); private final DoubleSupplier voltageSupplier; } + // 1 = up, -1 = down + private double voltageMultiplier = 1; + private ClimberState goalState = ClimberState.STOW; private ClimberState lastState; @@ -60,6 +66,9 @@ public enum ClimberState implements VoltageState { private final ClimberIOInputsAutoLogged inputs = new ClimberIOInputsAutoLogged(); + private static final LoggedTunableNumber laserCanDetectThreshold = + new LoggedTunableNumber("ClimberIOTalonFX/LaserCAN/DetectThreshold", 20); + public Climber(ClimberIO io) { this.io = io; @@ -77,10 +86,9 @@ public void periodic() { if (getGoalState() != lastState) { stateTimer.reset(); lastState = getGoalState(); - System.out.println("New goal state: " + getGoalState().name()); } - io.runVolts(goalState.getVoltageSupplier().getAsDouble()); + io.runVolts(goalState.getVoltageSupplier().getAsDouble() * voltageMultiplier); Logger.recordOutput("Climber/Climber Goal", goalState.toString()); } @@ -99,6 +107,14 @@ public void setGoalState(ClimberState goalState) { } } + /** + * @return true if cage is detected by climber LaserCAN + */ + public boolean isCageInClimber() { + return inputs.laserCanMeasurement.getDistance() + < (int) Math.floor(laserCanDetectThreshold.get()); + } + public void runVolts(double volts) { io.runVolts(volts); } diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java index 77b61ef..f124bcc 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java @@ -17,11 +17,11 @@ package org.team5924.frc2025.subsystems.climber; import org.littletonrobotics.junction.AutoLog; +import org.team5924.frc2025.util.LaserCAN_Measurement; public interface ClimberIO { @AutoLog public static class ClimberIOInputs { - public boolean motorConnected = true; public double positionRads = 0.0; public double velocityRadsPerSec = 0.0; @@ -29,6 +29,10 @@ public static class ClimberIOInputs { public double supplyCurrentAmps = 0.0; public double torqueCurrentAmps = 0.0; public double tempCelsius = 0.0; + + // Climber LaserCAN + public LaserCAN_Measurement laserCanMeasurement = new LaserCAN_Measurement(); + public boolean laserCanConnected = true; } public default void updateInputs(ClimberIOInputs inputs) {} diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java index 08a3210..72e8a98 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java @@ -18,6 +18,7 @@ import static edu.wpi.first.units.Units.Radians; +import au.grapplerobotics.LaserCan; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -32,7 +33,12 @@ import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import org.littletonrobotics.junction.Logger; import org.team5924.frc2025.Constants; +import org.team5924.frc2025.util.LaserCAN_Measurement; +import org.team5924.frc2025.util.exceptions.SensorRuntimeException; /** Add your docs here. */ public class ClimberIOTalonFX implements ClimberIO { @@ -52,6 +58,13 @@ public class ClimberIOTalonFX implements ClimberIO { private final double reduction; + private static final LaserCan laserCan = new LaserCan(Constants.CLIMBER_LASER_CAN_ID); + + private static final Alert laserCanDisconnectAlert = + new Alert("Climber LaserCAN disconnected.", AlertType.kWarning); + private static final Alert laserCanInvalidMeasure = + new Alert("Climber LaserCAN grabbed invalid measurement. See logs.", AlertType.kWarning); + public ClimberIOTalonFX() { reduction = Constants.SHOOTER_REDUCTION; talon = new TalonFX(Constants.SHOOTER_CAN_ID, Constants.SHOOTER_BUS); @@ -84,6 +97,25 @@ public ClimberIOTalonFX() { @Override public void updateInputs(ClimberIOInputs inputs) { + try { + inputs.laserCanMeasurement = LaserCAN_Measurement.fromLaserCAN(laserCan.getMeasurement()); + inputs.laserCanConnected = true; + laserCanDisconnectAlert.set(false); + laserCanInvalidMeasure.set(false); + } catch (SensorRuntimeException e) { + switch (e.getErrorType()) { + case DISCONNECTED -> { + inputs.laserCanConnected = false; + laserCanDisconnectAlert.set(true); + } + case INVALID_DATA -> laserCanInvalidMeasure.set(true); + default -> { + if (Constants.ALLOW_ASSERTS) throw e; + else System.err.println("FIX NOW: Unhandled SensorRuntimeException: " + e.getMessage()); + } + } + } + inputs.motorConnected = BaseStatusSignal.refreshAll( position, velocity, appliedVoltage, supplyCurrent, torqueCurrent, tempCelsius) @@ -103,6 +135,14 @@ public void runVolts(double volts) { @Override public void setAngle(double rads) { + if (rads < Constants.CLIMBER_MIN_RADS || rads > Constants.CLIMBER_MAX_RADS) { + Logger.recordOutput( + "Climber/InvalidAngle", + "Cannot set climber angle to " + + rads + + " rads. This value extends past the climber angle boundary."); + } + rads = Math.min(Constants.CLIMBER_MIN_RADS, Math.max(rads, Constants.CLIMBER_MAX_RADS)); talon.setControl(positionOut.withPosition(Radians.of(rads))); } } From 50a2fa24d5cc98f5f835b9e1a349fa0f413a2fd8 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Mon, 10 Feb 2025 10:31:51 -0800 Subject: [PATCH 05/11] add javadoc comments to functions in ClimberIO. setAngle now throws an IllegalArgumentException if rads is not within bounds --- .../org/team5924/frc2025/BuildConstants.java | 10 +++++----- .../org/team5924/frc2025/RobotContainer.java | 2 +- .../frc2025/subsystems/climber/ClimberIO.java | 19 +++++++++++++++++++ .../subsystems/climber/ClimberIOTalonFX.java | 9 +++++---- 4 files changed, 30 insertions(+), 10 deletions(-) diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index f2ee9d7..3a6ca30 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,12 +21,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 36; - public static final String GIT_SHA = "942123efc9cbba4ae597ef7383db5a5d7b0f82a9"; - public static final String GIT_DATE = "2025-02-04 19:59:22 EST"; + public static final int GIT_REVISION = 37; + public static final String GIT_SHA = "c956f7176da2e91028bb9def655178e3d537d01b"; + public static final String GIT_DATE = "2025-02-07 22:36:23 EST"; public static final String GIT_BRANCH = "7-climber-basic-functionality"; - public static final String BUILD_DATE = "2025-02-07 22:19:33 EST"; - public static final long BUILD_UNIX_TIME = 1738984773492L; + public static final String BUILD_DATE = "2025-02-10 13:28:33 EST"; + public static final long BUILD_UNIX_TIME = 1739212113549L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index 959018e..0187eb3 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -177,7 +177,7 @@ private void configureButtonBindings() { .onTrue( Commands.runOnce(() -> coralInAndOut.setGoalState(CoralInAndOut.CoralState.LOADING))); - // Climber + // # Climber # // Dpad Down driveController diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java index f124bcc..d3009e8 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java @@ -17,6 +17,7 @@ package org.team5924.frc2025.subsystems.climber; import org.littletonrobotics.junction.AutoLog; +import org.team5924.frc2025.Constants; import org.team5924.frc2025.util.LaserCAN_Measurement; public interface ClimberIO { @@ -35,11 +36,29 @@ public static class ClimberIOInputs { public boolean laserCanConnected = true; } + /** + * Updates the inputs object with the latest data from hardware + * + * @param inputs Inputs to update + */ public default void updateInputs(ClimberIOInputs inputs) {} + /** + * Runs the motor at the specified voltage + * + * @param volts Voltage to apply + */ public default void runVolts(double volts) {} + /** + * Sets the target angle for the climber + * + * @param rads Target angle in radians, must be between {@link Constants}.CLIMBER_MIN_RADS and + * {@link Constants}.CLIMBER_MAX_RADS + * @throws IllegalArgumentException if value does not fall in range + */ public default void setAngle(double rads) {} + /** stops the motor */ default void stop() {} } diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java index 72e8a98..5031aed 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java @@ -134,13 +134,14 @@ public void runVolts(double volts) { } @Override - public void setAngle(double rads) { + public void setAngle(double rads) throws IllegalArgumentException { if (rads < Constants.CLIMBER_MIN_RADS || rads > Constants.CLIMBER_MAX_RADS) { - Logger.recordOutput( - "Climber/InvalidAngle", + String message = "Cannot set climber angle to " + rads - + " rads. This value extends past the climber angle boundary."); + + " rads. This value extends past the climber angle boundary."; + Logger.recordOutput("Climber/InvalidAngle", message); + throw new IllegalArgumentException(message); } rads = Math.min(Constants.CLIMBER_MIN_RADS, Math.max(rads, Constants.CLIMBER_MAX_RADS)); talon.setControl(positionOut.withPosition(Radians.of(rads))); From 7607f18b60692d738fc19d383592565d79d24913 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 11 Feb 2025 19:50:38 -0800 Subject: [PATCH 06/11] add Climber state transition validation, add transition from STOW to READY_TO_CLIMB state once cage is within range, fix usage and description of CLIMB state --- .../org/team5924/frc2025/BuildConstants.java | 10 +++--- .../org/team5924/frc2025/RobotContainer.java | 2 +- .../frc2025/subsystems/climber/Climber.java | 36 ++++++++++++++++--- 3 files changed, 38 insertions(+), 10 deletions(-) diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index 3a6ca30..a2abe95 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,12 +21,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 37; - public static final String GIT_SHA = "c956f7176da2e91028bb9def655178e3d537d01b"; - public static final String GIT_DATE = "2025-02-07 22:36:23 EST"; + public static final int GIT_REVISION = 38; + public static final String GIT_SHA = "50a2fa24d5cc98f5f835b9e1a349fa0f413a2fd8"; + public static final String GIT_DATE = "2025-02-10 13:31:51 EST"; public static final String GIT_BRANCH = "7-climber-basic-functionality"; - public static final String BUILD_DATE = "2025-02-10 13:28:33 EST"; - public static final long BUILD_UNIX_TIME = 1739212113549L; + public static final String BUILD_DATE = "2025-02-11 22:46:49 EST"; + public static final long BUILD_UNIX_TIME = 1739332009026L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index 0187eb3..1b47309 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -197,7 +197,7 @@ private void configureButtonBindings() { driveController .pov(180) .or(driveController.pov(0)) - .onFalse(Commands.runOnce(() -> climber.setGoalState(Climber.ClimberState.STOW))); + .onFalse(Commands.runOnce(() -> climber.setGoalStateToNotMoving())); } /** diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java b/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java index 60aa810..bd24e18 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java @@ -16,6 +16,7 @@ package org.team5924.frc2025.subsystems.climber; +import au.grapplerobotics.LaserCan; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -38,7 +39,7 @@ public interface VoltageState { @RequiredArgsConstructor @Getter public enum ClimberState implements VoltageState { - // Finished climbing? + // In the process of climbing, not moving CLIMB(new LoggedTunableNumber("Climber/ClimbingVoltage", 0.0)), // Tucked in @@ -47,7 +48,7 @@ public enum ClimberState implements VoltageState { // Ready to climb READY_TO_CLIMB(new LoggedTunableNumber("Climber/ReadyToClimbVoltage", 0.0)), - // Climber is moving down + // Climber is moving down/up depending on voltage multiplier MOVING(new LoggedTunableNumber("Climber/MovingVoltage", 12.0)); private final DoubleSupplier voltageSupplier; @@ -78,11 +79,18 @@ public Climber(ClimberIO io) { @Override public void periodic() { + System.out.println("Current climber goal state: " + getGoalState().name()); io.updateInputs(inputs); Logger.processInputs("Climber", inputs); disconnected.set(!inputs.motorConnected); + if (getGoalState() == ClimberState.STOW && isCageInClimber()) { + setGoalState( + ClimberState.READY_TO_CLIMB); // if the cage is within range and the robot's state is + // STOW, then set the robot's state to READY_TO_CLIMB + } + if (getGoalState() != lastState) { stateTimer.reset(); lastState = getGoalState(); @@ -98,6 +106,17 @@ public void periodic() { * @param goalState the new goal state */ public void setGoalState(ClimberState goalState) { + // Validate state transitions + if (getGoalState() == ClimberState.STOW + && (goalState == ClimberState.CLIMB || goalState == ClimberState.MOVING)) { + Logger.recordOutput( + "Climber/InvalidTransition", + "Cannot transition from STOW to " + + goalState.name() + + "; robot needs to be READY_TO_CLIMB before performing any climbing action"); + return; + } + this.goalState = goalState; switch (goalState) { case CLIMB -> RobotState.getInstance().setClimberState(ClimberState.CLIMB); @@ -107,12 +126,21 @@ public void setGoalState(ClimberState goalState) { } } + /** Sets the goal state of the climber when Dpad Up or Dpad Down is not pressed. */ + public void setGoalStateToNotMoving() { + switch (getGoalState()) { + case MOVING -> setGoalState(ClimberState.CLIMB); // the climber stopped moving + default -> {} // the climber was not moving in the first place; don't need a state change here + } + } + /** * @return true if cage is detected by climber LaserCAN */ public boolean isCageInClimber() { - return inputs.laserCanMeasurement.getDistance() - < (int) Math.floor(laserCanDetectThreshold.get()); + return inputs.laserCanMeasurement.getStatus() == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT + && inputs.laserCanMeasurement.getDistance() + < (int) Math.floor(laserCanDetectThreshold.get()); } public void runVolts(double volts) { From 34890559e0f848850bc19b57392d215e8e818e0f Mon Sep 17 00:00:00 2001 From: James Guleno Date: Tue, 11 Feb 2025 20:03:53 -0800 Subject: [PATCH 07/11] resolve review fixes (remove println in climber periodic), clarify some javadoc comments --- .../java/org/team5924/frc2025/BuildConstants.java | 10 +++++----- .../frc2025/subsystems/climber/Climber.java | 15 +++++++++------ 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index a2abe95..b240f53 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,12 +21,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 38; - public static final String GIT_SHA = "50a2fa24d5cc98f5f835b9e1a349fa0f413a2fd8"; - public static final String GIT_DATE = "2025-02-10 13:31:51 EST"; + public static final int GIT_REVISION = 39; + public static final String GIT_SHA = "7607f18b60692d738fc19d383592565d79d24913"; + public static final String GIT_DATE = "2025-02-11 22:50:38 EST"; public static final String GIT_BRANCH = "7-climber-basic-functionality"; - public static final String BUILD_DATE = "2025-02-11 22:46:49 EST"; - public static final long BUILD_UNIX_TIME = 1739332009026L; + public static final String BUILD_DATE = "2025-02-11 23:02:05 EST"; + public static final long BUILD_UNIX_TIME = 1739332925618L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java b/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java index bd24e18..b9e4827 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java @@ -39,7 +39,7 @@ public interface VoltageState { @RequiredArgsConstructor @Getter public enum ClimberState implements VoltageState { - // In the process of climbing, not moving + // In the process of climbing, but not currently moving CLIMB(new LoggedTunableNumber("Climber/ClimbingVoltage", 0.0)), // Tucked in @@ -68,7 +68,7 @@ public enum ClimberState implements VoltageState { private final ClimberIOInputsAutoLogged inputs = new ClimberIOInputsAutoLogged(); private static final LoggedTunableNumber laserCanDetectThreshold = - new LoggedTunableNumber("ClimberIOTalonFX/LaserCAN/DetectThreshold", 20); + new LoggedTunableNumber("Climber/LaserCAN/DetectThreshold", 20); public Climber(ClimberIO io) { this.io = io; @@ -79,7 +79,6 @@ public Climber(ClimberIO io) { @Override public void periodic() { - System.out.println("Current climber goal state: " + getGoalState().name()); io.updateInputs(inputs); Logger.processInputs("Climber", inputs); @@ -126,11 +125,15 @@ public void setGoalState(ClimberState goalState) { } } - /** Sets the goal state of the climber when Dpad Up or Dpad Down is not pressed. */ + /** + * Sets the goal state of the climber when it isn't moving (neither Dpad Up nor Dpad Down is + * pressed). + */ public void setGoalStateToNotMoving() { switch (getGoalState()) { - case MOVING -> setGoalState(ClimberState.CLIMB); // the climber stopped moving - default -> {} // the climber was not moving in the first place; don't need a state change here + case MOVING -> + setGoalState(ClimberState.CLIMB); // the climber stopped moving, return to climb state + default -> {} // the climber was not moving in the first place, don't need a state change here } } From e80f421cf0bd5caf72516f8f350cb3beadd40484 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Fri, 14 Feb 2025 20:11:37 -0800 Subject: [PATCH 08/11] add driver station error reporting to invalid ClimberState transitions, use LoggedTunableNumber instead of DoubleSupplier for ClimberState volts --- .../org/team5924/frc2025/BuildConstants.java | 66 ++-- .../frc2025/subsystems/climber/Climber.java | 310 +++++++++--------- 2 files changed, 187 insertions(+), 189 deletions(-) diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index b240f53..3df3e1a 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -1,33 +1,33 @@ -/* - * BuildConstants.java - */ - -/* - * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2025; - -/** Automatically generated file containing build version information. */ -public final class BuildConstants { - public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "GoldenGateRobotics2025"; - public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 39; - public static final String GIT_SHA = "7607f18b60692d738fc19d383592565d79d24913"; - public static final String GIT_DATE = "2025-02-11 22:50:38 EST"; - public static final String GIT_BRANCH = "7-climber-basic-functionality"; - public static final String BUILD_DATE = "2025-02-11 23:02:05 EST"; - public static final long BUILD_UNIX_TIME = 1739332925618L; - public static final int DIRTY = 1; - - private BuildConstants() {} -} +/* + * BuildConstants.java + */ + +/* + * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2025; + +/** Automatically generated file containing build version information. */ +public final class BuildConstants { + public static final String MAVEN_GROUP = ""; + public static final String MAVEN_NAME = "GoldenGateRobotics2025"; + public static final String VERSION = "unspecified"; + public static final int GIT_REVISION = 40; + public static final String GIT_SHA = "34890559e0f848850bc19b57392d215e8e818e0f"; + public static final String GIT_DATE = "2025-02-11 23:03:53 EST"; + public static final String GIT_BRANCH = "7-climber-basic-functionality"; + public static final String BUILD_DATE = "2025-02-14 23:03:55 EST"; + public static final long BUILD_UNIX_TIME = 1739592235876L; + public static final int DIRTY = 1; + + private BuildConstants() {} +} diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java b/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java index b9e4827..86e0459 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java @@ -1,156 +1,154 @@ -/* - * Climber.java - */ - -/* - * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2025.subsystems.climber; - -import au.grapplerobotics.LaserCan; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.function.DoubleSupplier; -import lombok.Getter; -import lombok.RequiredArgsConstructor; -import lombok.Setter; -import org.littletonrobotics.junction.Logger; -import org.team5924.frc2025.RobotState; -import org.team5924.frc2025.util.LoggedTunableNumber; - -@Setter -@Getter -public class Climber extends SubsystemBase { - - public interface VoltageState { - DoubleSupplier getVoltageSupplier(); - } - - @RequiredArgsConstructor - @Getter - public enum ClimberState implements VoltageState { - // In the process of climbing, but not currently moving - CLIMB(new LoggedTunableNumber("Climber/ClimbingVoltage", 0.0)), - - // Tucked in - STOW(new LoggedTunableNumber("Climber/StowVoltage", 0.0)), - - // Ready to climb - READY_TO_CLIMB(new LoggedTunableNumber("Climber/ReadyToClimbVoltage", 0.0)), - - // Climber is moving down/up depending on voltage multiplier - MOVING(new LoggedTunableNumber("Climber/MovingVoltage", 12.0)); - - private final DoubleSupplier voltageSupplier; - } - - // 1 = up, -1 = down - private double voltageMultiplier = 1; - - private ClimberState goalState = ClimberState.STOW; - private ClimberState lastState; - - private final Alert disconnected; - protected final Timer stateTimer = new Timer(); - - @Getter private final ClimberIO io; - - private final ClimberIOInputsAutoLogged inputs = new ClimberIOInputsAutoLogged(); - - private static final LoggedTunableNumber laserCanDetectThreshold = - new LoggedTunableNumber("Climber/LaserCAN/DetectThreshold", 20); - - public Climber(ClimberIO io) { - this.io = io; - - disconnected = new Alert("Climber disconnected!", Alert.AlertType.kWarning); - stateTimer.start(); - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Climber", inputs); - - disconnected.set(!inputs.motorConnected); - - if (getGoalState() == ClimberState.STOW && isCageInClimber()) { - setGoalState( - ClimberState.READY_TO_CLIMB); // if the cage is within range and the robot's state is - // STOW, then set the robot's state to READY_TO_CLIMB - } - - if (getGoalState() != lastState) { - stateTimer.reset(); - lastState = getGoalState(); - } - - io.runVolts(goalState.getVoltageSupplier().getAsDouble() * voltageMultiplier); - Logger.recordOutput("Climber/Climber Goal", goalState.toString()); - } - - /** - * Sets the goal state of the climber. - * - * @param goalState the new goal state - */ - public void setGoalState(ClimberState goalState) { - // Validate state transitions - if (getGoalState() == ClimberState.STOW - && (goalState == ClimberState.CLIMB || goalState == ClimberState.MOVING)) { - Logger.recordOutput( - "Climber/InvalidTransition", - "Cannot transition from STOW to " - + goalState.name() - + "; robot needs to be READY_TO_CLIMB before performing any climbing action"); - return; - } - - this.goalState = goalState; - switch (goalState) { - case CLIMB -> RobotState.getInstance().setClimberState(ClimberState.CLIMB); - case STOW -> RobotState.getInstance().setClimberState(ClimberState.STOW); - case READY_TO_CLIMB -> RobotState.getInstance().setClimberState(ClimberState.READY_TO_CLIMB); - case MOVING -> RobotState.getInstance().setClimberState(ClimberState.MOVING); - } - } - - /** - * Sets the goal state of the climber when it isn't moving (neither Dpad Up nor Dpad Down is - * pressed). - */ - public void setGoalStateToNotMoving() { - switch (getGoalState()) { - case MOVING -> - setGoalState(ClimberState.CLIMB); // the climber stopped moving, return to climb state - default -> {} // the climber was not moving in the first place, don't need a state change here - } - } - - /** - * @return true if cage is detected by climber LaserCAN - */ - public boolean isCageInClimber() { - return inputs.laserCanMeasurement.getStatus() == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT - && inputs.laserCanMeasurement.getDistance() - < (int) Math.floor(laserCanDetectThreshold.get()); - } - - public void runVolts(double volts) { - io.runVolts(volts); - } - - public void setAngle(double rads) { - io.setAngle(rads); - } -} +/* + * Climber.java + */ + +/* + * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2025.subsystems.climber; + +import au.grapplerobotics.LaserCan; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import lombok.Getter; +import lombok.Setter; +import org.littletonrobotics.junction.Logger; +import org.team5924.frc2025.RobotState; +import org.team5924.frc2025.util.LoggedTunableNumber; + +@Setter +@Getter +public class Climber extends SubsystemBase { + public enum ClimberState { + // In the process of climbing, but not currently moving + CLIMB(new LoggedTunableNumber("Climber/ClimbingVoltage", 0.0)), + + // Tucked in + STOW(new LoggedTunableNumber("Climber/StowVoltage", 0.0)), + + // Ready to climb + READY_TO_CLIMB(new LoggedTunableNumber("Climber/ReadyToClimbVoltage", 0.0)), + + // Climber is moving down/up depending on voltage multiplier + MOVING(new LoggedTunableNumber("Climber/MovingVoltage", 12.0)); + + private final LoggedTunableNumber volts; + + ClimberState(LoggedTunableNumber volts) { + this.volts = volts; + } + } + + // 1 = up, -1 = down + private double voltageMultiplier = 1; + + private ClimberState goalState = ClimberState.STOW; + private ClimberState lastState; + + private final Alert disconnected; + protected final Timer stateTimer = new Timer(); + + private final ClimberIO io; + + private final ClimberIOInputsAutoLogged inputs = new ClimberIOInputsAutoLogged(); + + private static final LoggedTunableNumber laserCanDetectThreshold = + new LoggedTunableNumber("Climber/LaserCAN/DetectThreshold", 20); + + public Climber(ClimberIO io) { + this.io = io; + + disconnected = new Alert("Climber disconnected!", Alert.AlertType.kWarning); + stateTimer.start(); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Climber", inputs); + + disconnected.set(!inputs.motorConnected); + + // If the robot's state is STOW and the cage is within range, then set the robot's state to + // READY_TO_CLIMB + if (getGoalState() == ClimberState.STOW && isCageInClimber()) { + setGoalState(ClimberState.READY_TO_CLIMB); + } + + if (getGoalState() != lastState) { + stateTimer.reset(); + lastState = getGoalState(); + } + + io.runVolts(goalState.volts.getAsDouble() * voltageMultiplier); + Logger.recordOutput("Climber/Climber Goal", goalState.toString()); + } + + /** + * Sets the goal state of the climber. + * + * @param goalState the new goal state + */ + public void setGoalState(ClimberState goalState) { + // Validate state transitions + if (getGoalState() == ClimberState.STOW + && (goalState == ClimberState.CLIMB || goalState == ClimberState.MOVING)) { + DriverStation.reportError( + "Cannot transition Climber from STOW to " + + goalState.name() + + ". Robot needs to be READY_TO_CLIMB before performing any climbing action", + new StackTraceElement[] { + new StackTraceElement("Climber", "setGoalState", "Climber", 106) + }); + return; + } + + this.goalState = goalState; + switch (goalState) { + case CLIMB -> RobotState.getInstance().setClimberState(ClimberState.CLIMB); + case STOW -> RobotState.getInstance().setClimberState(ClimberState.STOW); + case READY_TO_CLIMB -> RobotState.getInstance().setClimberState(ClimberState.READY_TO_CLIMB); + case MOVING -> RobotState.getInstance().setClimberState(ClimberState.MOVING); + } + } + + /** + * Sets the goal state of the climber when it isn't moving (neither Dpad Up nor Dpad Down is + * pressed). + */ + public void setGoalStateToNotMoving() { + switch (getGoalState()) { + case MOVING -> + setGoalState(ClimberState.CLIMB); // the climber stopped moving, return to climb state + default -> {} // the climber was not moving in the first place, don't need a state change here + } + } + + /** + * @return true if cage is detected by climber LaserCAN + */ + public boolean isCageInClimber() { + return inputs.laserCanMeasurement.getStatus() == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT + && inputs.laserCanMeasurement.getDistance() + < (int) Math.floor(laserCanDetectThreshold.get()); + } + + public void runVolts(double volts) { + io.runVolts(volts); + } + + public void setAngle(double rads) { + io.setAngle(rads); + } +} From 4fa3c5482e083ec03abba22211f04b6b0e0aae69 Mon Sep 17 00:00:00 2001 From: James Guleno Date: Sun, 16 Feb 2025 16:46:40 -0800 Subject: [PATCH 09/11] add checking for elevator and pivot in stow before ready to climb, climber stops when no input on dpad. Rename Algae folder to pivot to resolve errors --- .vscode/settings.json | 2 +- .../org/team5924/frc2025/BuildConstants.java | 10 +++++----- .../java/org/team5924/frc2025/Constants.java | 6 +++--- .../org/team5924/frc2025/RobotContainer.java | 12 ++++++++++++ .../java/org/team5924/frc2025/RobotState.java | 2 +- .../frc2025/subsystems/climber/Climber.java | 17 +++++++++++------ .../subsystems/climber/ClimberIOSim.java | 3 ++- .../subsystems/{Algae => pivot}/AlgaePivot.java | 2 +- .../{Algae => pivot}/AlgaePivotIO.java | 2 +- .../{Algae => pivot}/AlgaePivotIOTalonFX.java | 2 +- 10 files changed, 38 insertions(+), 20 deletions(-) rename src/main/java/org/team5924/frc2025/subsystems/{Algae => pivot}/AlgaePivot.java (98%) rename src/main/java/org/team5924/frc2025/subsystems/{Algae => pivot}/AlgaePivotIO.java (97%) rename src/main/java/org/team5924/frc2025/subsystems/{Algae => pivot}/AlgaePivotIOTalonFX.java (99%) diff --git a/.vscode/settings.json b/.vscode/settings.json index 71f3b3d..afcedd5 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -69,5 +69,5 @@ "[java]": { "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" }, - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable" } diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index 11300ac..65b082d 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,12 +21,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 41; - public static final String GIT_SHA = "e80f421cf0bd5caf72516f8f350cb3beadd40484"; - public static final String GIT_DATE = "2025-02-14 23:11:37 EST"; + public static final int GIT_REVISION = 95; + public static final String GIT_SHA = "8eeb3a7e241d2b4feceb7f61e7e26c6f8ab8f0bc"; + public static final String GIT_DATE = "2025-02-15 20:25:52 EST"; public static final String GIT_BRANCH = "7-climber-basic-functionality"; - public static final String BUILD_DATE = "2025-02-15 20:07:59 EST"; - public static final long BUILD_UNIX_TIME = 1739668079760L; + public static final String BUILD_DATE = "2025-02-16 19:36:22 EST"; + public static final long BUILD_UNIX_TIME = 1739752582751L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team5924/frc2025/Constants.java b/src/main/java/org/team5924/frc2025/Constants.java index 7b342d8..82b3db3 100644 --- a/src/main/java/org/team5924/frc2025/Constants.java +++ b/src/main/java/org/team5924/frc2025/Constants.java @@ -49,16 +49,16 @@ public static enum Mode { /* General */ public static final double LOOP_PERIODIC_SECONDS = 0.02; /* Climber */ - public static final int CLIMBER_CAN_ID = 0; // TODO: update ID + public static final int CLIMBER_CAN_ID = 40; public static final String CLIMBER_BUS = "rio"; public static final int CLIMBER_CURRENT_LIMIT = 40; public static final boolean CLIMBER_INVERT = false; public static final boolean CLIMBER_BRAKE = true; - public static final double CLIMBER_REDUCTION = 58.3; + public static final double CLIMBER_REDUCTION = 58.3; // TODO: correct for now, check again later public static final double CLIMBER_SIM_MOI = 0.001; public static final double CLIMBER_MIN_RADS = -Math.PI / 2; // TODO: get real min public static final double CLIMBER_MAX_RADS = Math.PI / 2; // TODO: get real max - public static final int CLIMBER_LASER_CAN_ID = 0; // TODO: update ID + public static final int CLIMBER_LASER_CAN_ID = 42; /* # Rollers # */ /* Coral In-And-Out */ diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index 174123b..4350184 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -223,6 +223,18 @@ private void configureButtonBindings() { .pov(0) .onTrue(Commands.runOnce(() -> climber.setGoalState(Climber.ClimberState.REVERSE_CLIMB))); // .finallyDo(() -> climber.setVoltageMultiplier(1))); + + // No Dpad Up or Dpad Down + driveController + .pov(180) + .or(driveController.pov(0)) + .onFalse( + Commands.runOnce( + () -> + climber.setGoalState( + climber.getGoalState() == Climber.ClimberState.STOW + ? Climber.ClimberState.STOW + : Climber.ClimberState.READY_TO_CLIMB))); } /** diff --git a/src/main/java/org/team5924/frc2025/RobotState.java b/src/main/java/org/team5924/frc2025/RobotState.java index f1c9022..d04fa37 100644 --- a/src/main/java/org/team5924/frc2025/RobotState.java +++ b/src/main/java/org/team5924/frc2025/RobotState.java @@ -20,9 +20,9 @@ import lombok.Getter; import lombok.Setter; import org.littletonrobotics.junction.AutoLogOutput; -import org.team5924.frc2025.subsystems.algae.AlgaePivot.AlgaePivotState; import org.team5924.frc2025.subsystems.climber.Climber.ClimberState; import org.team5924.frc2025.subsystems.elevator.Elevator.ElevatorState; +import org.team5924.frc2025.subsystems.pivot.AlgaePivot.AlgaePivotState; import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState; @Getter diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java b/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java index 8e49807..bbd628e 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java @@ -24,6 +24,8 @@ import lombok.Setter; import org.littletonrobotics.junction.Logger; import org.team5924.frc2025.RobotState; +import org.team5924.frc2025.subsystems.elevator.Elevator.ElevatorState; +import org.team5924.frc2025.subsystems.pivot.AlgaePivot.AlgaePivotState; import org.team5924.frc2025.util.LoggedTunableNumber; @Setter @@ -33,14 +35,14 @@ public enum ClimberState { // Pulling onto cage, lifting robot CLIMB(new LoggedTunableNumber("Climber/ClimbingVoltage", 12.0)), - // Tucked in + // Default state, will be here most of the match STOW(new LoggedTunableNumber("Climber/StowVoltage", 0.0)), // Ready to climb READY_TO_CLIMB(new LoggedTunableNumber("Climber/ReadyToClimbVoltage", 0.0)), // Lowering robot - REVERSE_CLIMB(new LoggedTunableNumber("Climber/MovingVoltage", -12.0)); + REVERSE_CLIMB(new LoggedTunableNumber("Climber/ReverseClimbingVoltage", -12.0)); private final LoggedTunableNumber volts; @@ -79,10 +81,13 @@ public void periodic() { disconnected.set(!inputs.motorConnected); - // If the robot's state is STOW and the cage is within range, then set the robot's state to - // READY_TO_CLIMB + // If the robot's state is STOW && the cage is within range && elevator + algae pivot are both + // stow, + // then set the robot's state to READY_TO_CLIMB if (getGoalState() == ClimberState.STOW - && isCageInClimber()) { // TODO: add elevator in stow, algae pivot to stow + && isCageInClimber() + && RobotState.getInstance().getElevatorState() == ElevatorState.INTAKE + && RobotState.getInstance().getAlgaePivotState() == AlgaePivotState.INTAKE_FLOOR) { setGoalState(ClimberState.READY_TO_CLIMB); } @@ -91,7 +96,7 @@ && isCageInClimber()) { // TODO: add elevator in stow, algae pivot to stow lastState = getGoalState(); } - // io.runVolts(goalState.volts.getAsDouble() * voltageMultiplier); + io.runVolts(goalState.volts.getAsDouble()); Logger.recordOutput("Climber/Climber Goal", goalState.toString()); } diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOSim.java b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOSim.java index d7b3790..2662b8e 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOSim.java +++ b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOSim.java @@ -31,7 +31,8 @@ public class ClimberIOSim implements ClimberIO { public ClimberIOSim() { motorSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(CLIMBER_GEARBOX, Constants.CLIMBER_REDUCTION, 0.001), + LinearSystemId.createDCMotorSystem( + CLIMBER_GEARBOX, Constants.CLIMBER_REDUCTION, Constants.CLIMBER_SIM_MOI), CLIMBER_GEARBOX); } diff --git a/src/main/java/org/team5924/frc2025/subsystems/Algae/AlgaePivot.java b/src/main/java/org/team5924/frc2025/subsystems/pivot/AlgaePivot.java similarity index 98% rename from src/main/java/org/team5924/frc2025/subsystems/Algae/AlgaePivot.java rename to src/main/java/org/team5924/frc2025/subsystems/pivot/AlgaePivot.java index 39e24f3..3d5d9ea 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/Algae/AlgaePivot.java +++ b/src/main/java/org/team5924/frc2025/subsystems/pivot/AlgaePivot.java @@ -14,7 +14,7 @@ * If you did not, see . */ -package org.team5924.frc2025.subsystems.algae; +package org.team5924.frc2025.subsystems.pivot; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; diff --git a/src/main/java/org/team5924/frc2025/subsystems/Algae/AlgaePivotIO.java b/src/main/java/org/team5924/frc2025/subsystems/pivot/AlgaePivotIO.java similarity index 97% rename from src/main/java/org/team5924/frc2025/subsystems/Algae/AlgaePivotIO.java rename to src/main/java/org/team5924/frc2025/subsystems/pivot/AlgaePivotIO.java index 26e024d..c1c0023 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/Algae/AlgaePivotIO.java +++ b/src/main/java/org/team5924/frc2025/subsystems/pivot/AlgaePivotIO.java @@ -14,7 +14,7 @@ * If you did not, see . */ -package org.team5924.frc2025.subsystems.algae; +package org.team5924.frc2025.subsystems.pivot; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/org/team5924/frc2025/subsystems/Algae/AlgaePivotIOTalonFX.java b/src/main/java/org/team5924/frc2025/subsystems/pivot/AlgaePivotIOTalonFX.java similarity index 99% rename from src/main/java/org/team5924/frc2025/subsystems/Algae/AlgaePivotIOTalonFX.java rename to src/main/java/org/team5924/frc2025/subsystems/pivot/AlgaePivotIOTalonFX.java index 2b83201..1ae31a5 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/Algae/AlgaePivotIOTalonFX.java +++ b/src/main/java/org/team5924/frc2025/subsystems/pivot/AlgaePivotIOTalonFX.java @@ -14,7 +14,7 @@ * If you did not, see . */ -package org.team5924.frc2025.subsystems.algae; +package org.team5924.frc2025.subsystems.pivot; import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Celsius; From 8529d044b1a0214ffa6870a5d05b6768dc58d5e6 Mon Sep 17 00:00:00 2001 From: Jack Doherty Date: Sat, 22 Feb 2025 03:53:20 -0800 Subject: [PATCH 10/11] added over temp alerts to drive and elevator motors --- .../java/org/team5924/frc2025/Constants.java | 1 + .../frc2025/subsystems/drive/Module.java | 13 +++++++++ .../frc2025/subsystems/drive/ModuleIO.java | 2 ++ .../subsystems/drive/ModuleIOTalonFX.java | 17 +++++++++-- .../frc2025/subsystems/elevator/Elevator.java | 29 ++++++++++++++----- .../rollers/CoralInAndOut/CoralInAndOut.java | 8 ++--- 6 files changed, 54 insertions(+), 16 deletions(-) diff --git a/src/main/java/org/team5924/frc2025/Constants.java b/src/main/java/org/team5924/frc2025/Constants.java index 5fe0ab8..4aef63d 100644 --- a/src/main/java/org/team5924/frc2025/Constants.java +++ b/src/main/java/org/team5924/frc2025/Constants.java @@ -47,6 +47,7 @@ public static enum Mode { /* General */ public static final double LOOP_PERIODIC_SECONDS = 0.02; + public static final double MOTOR_MAX_TEMP = 95; /* ### Subsystems ### */ /* # Rollers # */ diff --git a/src/main/java/org/team5924/frc2025/subsystems/drive/Module.java b/src/main/java/org/team5924/frc2025/subsystems/drive/Module.java index 0eaff9c..f5919ea 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/drive/Module.java +++ b/src/main/java/org/team5924/frc2025/subsystems/drive/Module.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import org.littletonrobotics.junction.Logger; +import org.team5924.frc2025.Constants; public class Module { private final ModuleIO io; @@ -38,6 +39,8 @@ public class Module { private final Alert driveDisconnectedAlert; private final Alert turnDisconnectedAlert; private final Alert turnEncoderDisconnectedAlert; + private final Alert driveOverTempAlert; + private final Alert turnOverTempAlert; private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; public Module( @@ -59,6 +62,14 @@ public Module( new Alert( "Disconnected turn encoder on module " + Integer.toString(index) + ".", AlertType.kError); + driveOverTempAlert = + new Alert( + "Drive motor on module " + Integer.toString(index) + " is overheating.", + AlertType.kWarning); + turnOverTempAlert = + new Alert( + "Turn motor on module " + Integer.toString(index) + " is overheating.", + AlertType.kWarning); } public void periodic() { @@ -78,6 +89,8 @@ public void periodic() { driveDisconnectedAlert.set(!inputs.driveConnected); turnDisconnectedAlert.set(!inputs.turnConnected); turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); + driveOverTempAlert.set(inputs.driveTempCelsius > Constants.MOTOR_MAX_TEMP); + turnOverTempAlert.set(inputs.turnTempCelsius > Constants.MOTOR_MAX_TEMP); } /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ diff --git a/src/main/java/org/team5924/frc2025/subsystems/drive/ModuleIO.java b/src/main/java/org/team5924/frc2025/subsystems/drive/ModuleIO.java index 726a313..acb5307 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/drive/ModuleIO.java +++ b/src/main/java/org/team5924/frc2025/subsystems/drive/ModuleIO.java @@ -27,6 +27,7 @@ public static class ModuleIOInputs { public double driveVelocityRadPerSec = 0.0; public double driveAppliedVolts = 0.0; public double driveCurrentAmps = 0.0; + public double driveTempCelsius = 0.0; public boolean turnConnected = false; public boolean turnEncoderConnected = false; @@ -35,6 +36,7 @@ public static class ModuleIOInputs { public double turnVelocityRadPerSec = 0.0; public double turnAppliedVolts = 0.0; public double turnCurrentAmps = 0.0; + public double turnTempCelsius = 0.0; public double[] odometryTimestamps = new double[] {}; public double[] odometryDrivePositionsRad = new double[] {}; diff --git a/src/main/java/org/team5924/frc2025/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/org/team5924/frc2025/subsystems/drive/ModuleIOTalonFX.java index e6c8cdb..af0831e 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/org/team5924/frc2025/subsystems/drive/ModuleIOTalonFX.java @@ -42,6 +42,7 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Temperature; import edu.wpi.first.units.measure.Voltage; import java.util.Queue; import org.team5924.frc2025.generated.TunerConstants; @@ -83,6 +84,7 @@ public class ModuleIOTalonFX implements ModuleIO { private final StatusSignal driveVelocity; private final StatusSignal driveAppliedVolts; private final StatusSignal driveCurrent; + private final StatusSignal driveTemp; // Inputs from turn motor private final StatusSignal turnAbsolutePosition; @@ -91,6 +93,7 @@ public class ModuleIOTalonFX implements ModuleIO { private final StatusSignal turnVelocity; private final StatusSignal turnAppliedVolts; private final StatusSignal turnCurrent; + private final StatusSignal turnTemp; // Connection debouncers private final Debouncer driveConnectedDebounce = new Debouncer(0.5); @@ -167,6 +170,7 @@ public ModuleIOTalonFX( driveVelocity = driveTalon.getVelocity(); driveAppliedVolts = driveTalon.getMotorVoltage(); driveCurrent = driveTalon.getStatorCurrent(); + driveTemp = driveTalon.getDeviceTemp(); // Create turn status signals turnAbsolutePosition = cancoder.getAbsolutePosition(); @@ -175,6 +179,7 @@ public ModuleIOTalonFX( turnVelocity = turnTalon.getVelocity(); turnAppliedVolts = turnTalon.getMotorVoltage(); turnCurrent = turnTalon.getStatorCurrent(); + turnTemp = turnTalon.getDeviceTemp(); // Configure periodic frames BaseStatusSignal.setUpdateFrequencyForAll( @@ -187,7 +192,9 @@ public ModuleIOTalonFX( turnAbsolutePosition, turnVelocity, turnAppliedVolts, - turnCurrent); + turnCurrent, + driveTemp, + turnTemp); ParentDevice.optimizeBusUtilizationForAll(driveTalon, turnTalon); } @@ -195,9 +202,11 @@ public ModuleIOTalonFX( public void updateInputs(ModuleIOInputs inputs) { // Refresh all signals var driveStatus = - BaseStatusSignal.refreshAll(drivePosition, driveVelocity, driveAppliedVolts, driveCurrent); + BaseStatusSignal.refreshAll( + drivePosition, driveVelocity, driveAppliedVolts, driveCurrent, driveTemp); var turnStatus = - BaseStatusSignal.refreshAll(turnPosition, turnVelocity, turnAppliedVolts, turnCurrent); + BaseStatusSignal.refreshAll( + turnPosition, turnVelocity, turnAppliedVolts, turnCurrent, turnTemp); var turnEncoderStatus = BaseStatusSignal.refreshAll(turnAbsolutePosition); // Update drive inputs @@ -206,6 +215,7 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); inputs.driveCurrentAmps = driveCurrent.getValueAsDouble(); + inputs.driveTempCelsius = driveTemp.getValueAsDouble(); // Update turn inputs inputs.turnConnected = turnConnectedDebounce.calculate(turnStatus.isOK()); @@ -215,6 +225,7 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()); inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + inputs.turnTempCelsius = turnTemp.getValueAsDouble(); // Update odometry inputs inputs.odometryTimestamps = diff --git a/src/main/java/org/team5924/frc2025/subsystems/elevator/Elevator.java b/src/main/java/org/team5924/frc2025/subsystems/elevator/Elevator.java index 30e593f..561c77c 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/elevator/Elevator.java +++ b/src/main/java/org/team5924/frc2025/subsystems/elevator/Elevator.java @@ -16,21 +16,21 @@ package org.team5924.frc2025.subsystems.elevator; -import org.littletonrobotics.junction.Logger; -import org.team5924.frc2025.Constants; -import org.team5924.frc2025.RobotState; -import org.team5924.frc2025.util.LoggedTunableNumber; - import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; + import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import lombok.Getter; +import org.littletonrobotics.junction.Logger; +import org.team5924.frc2025.Constants; +import org.team5924.frc2025.RobotState; +import org.team5924.frc2025.util.LoggedTunableNumber; public class Elevator extends SubsystemBase { // Tolerance for position control (in meters) @@ -66,14 +66,22 @@ public enum ElevatorState { private final Alert leftMotorDisconnected; private final Alert rightMotorDisconnected; + private final Alert leftMotorOverTemp; + private final Alert rightMotorOverTemp; + public Elevator(ElevatorIO io) { this.io = io; this.goalState = ElevatorState.MANUAL; RobotState.getInstance().setElevatorState(this.goalState); this.leftMotorDisconnected = - new Alert("Left elevator motor disconnected!", Alert.AlertType.kWarning); + new Alert("Left elevator motor disconnected!", Alert.AlertType.kError); this.rightMotorDisconnected = - new Alert("Right elevator motor disconnected!", Alert.AlertType.kWarning); + new Alert("Right elevator motor disconnected!", Alert.AlertType.kError); + + this.leftMotorOverTemp = + new Alert("Left elevator motor over temperature!", Alert.AlertType.kWarning); + this.rightMotorOverTemp = + new Alert("Right elevator motor over temperature!", Alert.AlertType.kWarning); upSysId = new SysIdRoutine( @@ -107,6 +115,9 @@ public void periodic() { leftMotorDisconnected.set(!inputs.leftMotorConnected); rightMotorDisconnected.set(!inputs.rightMotorConnected); + leftMotorOverTemp.set(inputs.leftTempCelsius > Constants.MOTOR_MAX_TEMP); + rightMotorOverTemp.set(inputs.rightTempCelsius > Constants.MOTOR_MAX_TEMP); + io.periodicUpdates(); } @@ -139,7 +150,9 @@ public void setGoalState(ElevatorState goalState) { this.goalState = goalState; switch (goalState) { case MANUAL -> RobotState.getInstance().setElevatorState(ElevatorState.MANUAL); - case MOVING -> DriverStation.reportError("MOVING is an intermediate state and cannot be set as a goal state!", null); + case MOVING -> + DriverStation.reportError( + "MOVING is an intermediate state and cannot be set as a goal state!", null); default -> { RobotState.getInstance().setElevatorState(ElevatorState.MOVING); io.setHeight(goalState.heightMeters.getAsDouble()); diff --git a/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java b/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java index 36e46b2..1b146a1 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java +++ b/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java @@ -17,17 +17,15 @@ package org.team5924.frc2025.subsystems.rollers.CoralInAndOut; import java.util.function.DoubleSupplier; - +import lombok.Getter; +import lombok.RequiredArgsConstructor; +import lombok.Setter; import org.littletonrobotics.junction.Logger; import org.team5924.frc2025.RobotState; import org.team5924.frc2025.subsystems.rollers.GenericRollerSystem; import org.team5924.frc2025.subsystems.rollers.GenericRollerSystem.VoltageState; import org.team5924.frc2025.util.LoggedTunableNumber; -import lombok.Getter; -import lombok.RequiredArgsConstructor; -import lombok.Setter; - @Setter @Getter public class CoralInAndOut extends GenericRollerSystem { From 44e6e66beaf8eddb01eecdd20de9dba47b8006dc Mon Sep 17 00:00:00 2001 From: Jack Doherty Date: Fri, 28 Feb 2025 08:43:40 -0800 Subject: [PATCH 11/11] Revert "Merge branch '7-climber-basic-functionality' into 46-alerts" This reverts commit 255394e65df50be7476dcaa7589415255ce10588, reversing changes made to 8529d044b1a0214ffa6870a5d05b6768dc58d5e6. --- .vscode/settings.json | 3 +- .../java/org/team5924/frc2025/Constants.java | 14 -- .../org/team5924/frc2025/RobotContainer.java | 33 ---- .../java/org/team5924/frc2025/RobotState.java | 15 +- .../frc2025/subsystems/climber/Climber.java | 160 ------------------ .../frc2025/subsystems/climber/ClimberIO.java | 64 ------- .../subsystems/climber/ClimberIOSim.java | 62 ------- .../subsystems/climber/ClimberIOTalonFX.java | 149 ---------------- .../elevator/ElevatorIOTalonFX.java | 19 +-- 9 files changed, 14 insertions(+), 505 deletions(-) delete mode 100644 src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java delete mode 100644 src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java delete mode 100644 src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOSim.java delete mode 100644 src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java diff --git a/.vscode/settings.json b/.vscode/settings.json index afcedd5..868594c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -68,6 +68,5 @@ }, "[java]": { "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" - }, - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable" + } } diff --git a/src/main/java/org/team5924/frc2025/Constants.java b/src/main/java/org/team5924/frc2025/Constants.java index 2bc7b9b..4aef63d 100644 --- a/src/main/java/org/team5924/frc2025/Constants.java +++ b/src/main/java/org/team5924/frc2025/Constants.java @@ -50,20 +50,6 @@ public static enum Mode { public static final double MOTOR_MAX_TEMP = 95; /* ### Subsystems ### */ - /* General */ - public static final double LOOP_PERIODIC_SECONDS = 0.02; - /* Climber */ - public static final int CLIMBER_CAN_ID = 40; - public static final String CLIMBER_BUS = "rio"; - public static final int CLIMBER_CURRENT_LIMIT = 40; - public static final boolean CLIMBER_INVERT = false; - public static final boolean CLIMBER_BRAKE = true; - public static final double CLIMBER_REDUCTION = 58.3; // TODO: correct for now, check again later - public static final double CLIMBER_SIM_MOI = 0.001; - public static final double CLIMBER_MIN_RADS = -Math.PI / 2; // TODO: get real min - public static final double CLIMBER_MAX_RADS = Math.PI / 2; // TODO: get real max - public static final int CLIMBER_LASER_CAN_ID = 42; - /* # Rollers # */ /* Coral In-And-Out */ public static final int CORAL_IN_AND_OUT_CAN_ID = 33; diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index 804be48..42cd4e2 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -29,10 +29,6 @@ import org.team5924.frc2025.commands.DriveCommands; import org.team5924.frc2025.commands.elevator.RunElevator; import org.team5924.frc2025.generated.TunerConstants; -import org.team5924.frc2025.subsystems.climber.Climber; -import org.team5924.frc2025.subsystems.climber.ClimberIO; -import org.team5924.frc2025.subsystems.climber.ClimberIOSim; -import org.team5924.frc2025.subsystems.climber.ClimberIOTalonFX; import org.team5924.frc2025.subsystems.drive.Drive; import org.team5924.frc2025.subsystems.drive.GyroIO; import org.team5924.frc2025.subsystems.drive.GyroIOPigeon2; @@ -56,7 +52,6 @@ public class RobotContainer { // Subsystems private final Drive drive; - private final Climber climber; private final CoralInAndOut coralInAndOut; private final Elevator elevator; @@ -79,7 +74,6 @@ public RobotContainer() { new ModuleIOTalonFX(TunerConstants.FrontRight), new ModuleIOTalonFX(TunerConstants.BackLeft), new ModuleIOTalonFX(TunerConstants.BackRight)); - climber = new Climber(new ClimberIOTalonFX()); coralInAndOut = new CoralInAndOut(new CoralInAndOutIOKrakenFOC()); elevator = new Elevator(new ElevatorIOTalonFX() {}); break; @@ -93,7 +87,6 @@ public RobotContainer() { new ModuleIOSim(TunerConstants.FrontRight), new ModuleIOSim(TunerConstants.BackLeft), new ModuleIOSim(TunerConstants.BackRight)); - climber = new Climber(new ClimberIOSim()); coralInAndOut = new CoralInAndOut(new CoralInAndOutIOSim()); elevator = new Elevator(new ElevatorIO() {}); break; @@ -107,7 +100,6 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); - climber = new Climber(new ClimberIO() {}); coralInAndOut = new CoralInAndOut(new CoralInAndOutIO() {}); elevator = new Elevator(new ElevatorIO() {}); break; @@ -222,31 +214,6 @@ private void configureButtonBindings() { operatorController .leftBumper() .onTrue(Commands.runOnce(() -> elevator.setGoalState(Elevator.ElevatorState.MANUAL))); - - // Climber - // Dpad Down - driveController - .pov(180) - .onTrue(Commands.runOnce(() -> climber.setGoalState(Climber.ClimberState.CLIMB))); - // .finallyDo(() -> climber.setVoltageMultiplier(-1))); - - // Dpad Up - driveController - .pov(0) - .onTrue(Commands.runOnce(() -> climber.setGoalState(Climber.ClimberState.REVERSE_CLIMB))); - // .finallyDo(() -> climber.setVoltageMultiplier(1))); - - // No Dpad Up or Dpad Down - driveController - .pov(180) - .or(driveController.pov(0)) - .onFalse( - Commands.runOnce( - () -> - climber.setGoalState( - climber.getGoalState() == Climber.ClimberState.STOW - ? Climber.ClimberState.STOW - : Climber.ClimberState.READY_TO_CLIMB))); } /** diff --git a/src/main/java/org/team5924/frc2025/RobotState.java b/src/main/java/org/team5924/frc2025/RobotState.java index 69bd936..d9dfc42 100644 --- a/src/main/java/org/team5924/frc2025/RobotState.java +++ b/src/main/java/org/team5924/frc2025/RobotState.java @@ -16,14 +16,13 @@ package org.team5924.frc2025; -import org.littletonrobotics.junction.AutoLogOutput; -import org.team5924.frc2025.subsystems.climber.Climber.ClimberState; -import org.team5924.frc2025.subsystems.elevator.Elevator.ElevatorState; -import org.team5924.frc2025.subsystems.pivot.AlgaePivot.AlgaePivotState; - import edu.wpi.first.math.geometry.Pose2d; import lombok.Getter; import lombok.Setter; +import org.littletonrobotics.junction.AutoLogOutput; +import org.team5924.frc2025.subsystems.elevator.Elevator.ElevatorState; +import org.team5924.frc2025.subsystems.pivot.AlgaePivot.AlgaePivotState; +import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState; @Getter public class RobotState { @@ -41,12 +40,6 @@ public static RobotState getInstance() { @AutoLogOutput(key = "RobotState/EstimatedPose") private Pose2d estimatedPose = new Pose2d(); - /* ### Climber ### */ - @Setter - @AutoLogOutput(key = "RobotState/ClimberState") - private ClimberState climberState = ClimberState.STOW; - - @AutoLogOutput(key = "RobotState/ElevatorState") @Getter @Setter private ElevatorState elevatorState = ElevatorState.MANUAL; /* ### Coral In and Out ### */ diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java b/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java deleted file mode 100644 index bbd628e..0000000 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/Climber.java +++ /dev/null @@ -1,160 +0,0 @@ -/* - * Climber.java - */ - -/* - * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2025.subsystems.climber; - -import au.grapplerobotics.LaserCan; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import lombok.Getter; -import lombok.Setter; -import org.littletonrobotics.junction.Logger; -import org.team5924.frc2025.RobotState; -import org.team5924.frc2025.subsystems.elevator.Elevator.ElevatorState; -import org.team5924.frc2025.subsystems.pivot.AlgaePivot.AlgaePivotState; -import org.team5924.frc2025.util.LoggedTunableNumber; - -@Setter -@Getter -public class Climber extends SubsystemBase { - public enum ClimberState { - // Pulling onto cage, lifting robot - CLIMB(new LoggedTunableNumber("Climber/ClimbingVoltage", 12.0)), - - // Default state, will be here most of the match - STOW(new LoggedTunableNumber("Climber/StowVoltage", 0.0)), - - // Ready to climb - READY_TO_CLIMB(new LoggedTunableNumber("Climber/ReadyToClimbVoltage", 0.0)), - - // Lowering robot - REVERSE_CLIMB(new LoggedTunableNumber("Climber/ReverseClimbingVoltage", -12.0)); - - private final LoggedTunableNumber volts; - - ClimberState(LoggedTunableNumber volts) { - this.volts = volts; - } - } - - // 1 = up, -1 = down - // private double voltageMultiplier = 1; - - private ClimberState goalState = ClimberState.STOW; - private ClimberState lastState; - - private final Alert disconnected; - protected final Timer stateTimer = new Timer(); - - private final ClimberIO io; - - private final ClimberIOInputsAutoLogged inputs = new ClimberIOInputsAutoLogged(); - - private static final LoggedTunableNumber laserCanDetectThreshold = - new LoggedTunableNumber("Climber/LaserCAN/DetectThreshold", 20); - - public Climber(ClimberIO io) { - this.io = io; - - disconnected = new Alert("Climber disconnected!", Alert.AlertType.kWarning); - stateTimer.start(); - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Climber", inputs); - - disconnected.set(!inputs.motorConnected); - - // If the robot's state is STOW && the cage is within range && elevator + algae pivot are both - // stow, - // then set the robot's state to READY_TO_CLIMB - if (getGoalState() == ClimberState.STOW - && isCageInClimber() - && RobotState.getInstance().getElevatorState() == ElevatorState.INTAKE - && RobotState.getInstance().getAlgaePivotState() == AlgaePivotState.INTAKE_FLOOR) { - setGoalState(ClimberState.READY_TO_CLIMB); - } - - if (getGoalState() != lastState) { - stateTimer.reset(); - lastState = getGoalState(); - } - - io.runVolts(goalState.volts.getAsDouble()); - Logger.recordOutput("Climber/Climber Goal", goalState.toString()); - } - - /** - * Sets the goal state of the climber. - * - * @param goalState the new goal state - */ - public void setGoalState(ClimberState goalState) { - // Validate state transitions - // if (getGoalState() == ClimberState.STOW - // && (goalState == ClimberState.CLIMB || goalState == ClimberState.REVERSE_CLIMB)) { - // DriverStation.reportError( - // "Cannot transition Climber from STOW to " - // + goalState.name() - // + ". Robot needs to be READY_TO_CLIMB before performing any climbing action", - // new StackTraceElement[] { - // new StackTraceElement("Climber", "setGoalState", "Climber", 106) - // }); - // return; - // } - - this.goalState = goalState; - switch (goalState) { - case CLIMB -> RobotState.getInstance().setClimberState(ClimberState.CLIMB); - case STOW -> RobotState.getInstance().setClimberState(ClimberState.STOW); - case READY_TO_CLIMB -> RobotState.getInstance().setClimberState(ClimberState.READY_TO_CLIMB); - case REVERSE_CLIMB -> RobotState.getInstance().setClimberState(ClimberState.REVERSE_CLIMB); - } - } - - /** - * Sets the goal state of the climber when it isn't moving (neither Dpad Up nor Dpad Down is - * pressed). - */ - // public void setGoalStateToNotMoving() { - // switch (getGoalState()) { - // case MOVING -> - // setGoalState(ClimberState.CLIMB); // the climber stopped moving, return to climb state - // default -> {} // the climber was not moving in the first place, don't need a state change - // here - // } - // } - - /** - * @return true if cage is detected by climber LaserCAN - */ - public boolean isCageInClimber() { - return inputs.laserCanMeasurement.getStatus() == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT - && inputs.laserCanMeasurement.getDistance() - < (int) Math.floor(laserCanDetectThreshold.get()); - } - - public void runVolts(double volts) { - io.runVolts(volts); - } - - public void setAngle(double rads) { - io.setAngle(rads); - } -} diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java deleted file mode 100644 index d3009e8..0000000 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIO.java +++ /dev/null @@ -1,64 +0,0 @@ -/* - * ClimberIO.java - */ - -/* - * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2025.subsystems.climber; - -import org.littletonrobotics.junction.AutoLog; -import org.team5924.frc2025.Constants; -import org.team5924.frc2025.util.LaserCAN_Measurement; - -public interface ClimberIO { - @AutoLog - public static class ClimberIOInputs { - public boolean motorConnected = true; - public double positionRads = 0.0; - public double velocityRadsPerSec = 0.0; - public double appliedVoltage = 0.0; - public double supplyCurrentAmps = 0.0; - public double torqueCurrentAmps = 0.0; - public double tempCelsius = 0.0; - - // Climber LaserCAN - public LaserCAN_Measurement laserCanMeasurement = new LaserCAN_Measurement(); - public boolean laserCanConnected = true; - } - - /** - * Updates the inputs object with the latest data from hardware - * - * @param inputs Inputs to update - */ - public default void updateInputs(ClimberIOInputs inputs) {} - - /** - * Runs the motor at the specified voltage - * - * @param volts Voltage to apply - */ - public default void runVolts(double volts) {} - - /** - * Sets the target angle for the climber - * - * @param rads Target angle in radians, must be between {@link Constants}.CLIMBER_MIN_RADS and - * {@link Constants}.CLIMBER_MAX_RADS - * @throws IllegalArgumentException if value does not fall in range - */ - public default void setAngle(double rads) {} - - /** stops the motor */ - default void stop() {} -} diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOSim.java b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOSim.java deleted file mode 100644 index 2662b8e..0000000 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOSim.java +++ /dev/null @@ -1,62 +0,0 @@ -/* - * ClimberIOSim.java - */ - -/* - * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2025.subsystems.climber; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import org.team5924.frc2025.Constants; - -public class ClimberIOSim implements ClimberIO { - private final DCMotorSim motorSim; - private double appliedVoltage = 0.0; - private static final DCMotor CLIMBER_GEARBOX = DCMotor.getKrakenX60Foc(1); - - public ClimberIOSim() { - motorSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - CLIMBER_GEARBOX, Constants.CLIMBER_REDUCTION, Constants.CLIMBER_SIM_MOI), - CLIMBER_GEARBOX); - } - - @Override - public void updateInputs(ClimberIOInputs inputs) { - if (DriverStation.isDisabled()) { - runVolts(0.0); - } - - motorSim.update(Constants.LOOP_PERIODIC_SECONDS); - inputs.positionRads = motorSim.getAngularPositionRad(); - inputs.velocityRadsPerSec = motorSim.getAngularVelocityRadPerSec(); - inputs.appliedVoltage = appliedVoltage; - inputs.supplyCurrentAmps = motorSim.getCurrentDrawAmps(); - } - - @Override - public void runVolts(double volts) { - appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); - motorSim.setInputVoltage(appliedVoltage); - } - - @Override - public void stop() { - runVolts(0.0); - } -} diff --git a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java b/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java deleted file mode 100644 index 85c6159..0000000 --- a/src/main/java/org/team5924/frc2025/subsystems/climber/ClimberIOTalonFX.java +++ /dev/null @@ -1,149 +0,0 @@ -/* - * ClimberIOTalonFX.java - */ - -/* - * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2025.subsystems.climber; - -import static edu.wpi.first.units.Units.Radians; - -import au.grapplerobotics.LaserCan; -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Temperature; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; -import org.littletonrobotics.junction.Logger; -import org.team5924.frc2025.Constants; -import org.team5924.frc2025.util.LaserCAN_Measurement; -import org.team5924.frc2025.util.exceptions.SensorRuntimeException; - -/** Add your docs here. */ -public class ClimberIOTalonFX implements ClimberIO { - private final TalonFX talon; - - private final StatusSignal position; - private final StatusSignal velocity; - private final StatusSignal appliedVoltage; - private final StatusSignal supplyCurrent; - private final StatusSignal torqueCurrent; - private final StatusSignal tempCelsius; - - // Single shot for voltage mode, robot loop will call continuously - private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0); - private final PositionVoltage positionOut = - new PositionVoltage(0).withUpdateFreqHz(0.0).withEnableFOC(true); - - private final double reduction; - - private static final LaserCan laserCan = new LaserCan(Constants.CLIMBER_LASER_CAN_ID); - - private static final Alert laserCanDisconnectAlert = - new Alert("Climber LaserCAN disconnected.", AlertType.kWarning); - private static final Alert laserCanInvalidMeasure = - new Alert("Climber LaserCAN grabbed invalid measurement. See logs.", AlertType.kWarning); - - public ClimberIOTalonFX() { - reduction = Constants.CLIMBER_REDUCTION; - talon = new TalonFX(Constants.CLIMBER_CAN_ID, Constants.CLIMBER_BUS); - - // Configure TalonFX - TalonFXConfiguration config = new TalonFXConfiguration(); - config.MotorOutput.Inverted = - Constants.CLIMBER_INVERT - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - config.MotorOutput.NeutralMode = - Constants.CLIMBER_BRAKE ? NeutralModeValue.Brake : NeutralModeValue.Coast; - config.CurrentLimits.SupplyCurrentLimit = Constants.CLIMBER_CURRENT_LIMIT; - config.CurrentLimits.SupplyCurrentLimitEnable = true; - talon.getConfigurator().apply(config); - - // Get select status signals and set update frequency - position = talon.getPosition(); - velocity = talon.getVelocity(); - appliedVoltage = talon.getMotorVoltage(); - supplyCurrent = talon.getSupplyCurrent(); - torqueCurrent = talon.getTorqueCurrent(); - tempCelsius = talon.getDeviceTemp(); - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, position, velocity, appliedVoltage, supplyCurrent, torqueCurrent, tempCelsius); - - // Disables status signals not called for update above - talon.optimizeBusUtilization(0, 1.0); - } - - @Override - public void updateInputs(ClimberIOInputs inputs) { - try { - inputs.laserCanMeasurement = LaserCAN_Measurement.fromLaserCAN(laserCan.getMeasurement()); - inputs.laserCanConnected = true; - laserCanDisconnectAlert.set(false); - laserCanInvalidMeasure.set(false); - } catch (SensorRuntimeException e) { - switch (e.getErrorType()) { - case DISCONNECTED -> { - inputs.laserCanConnected = false; - laserCanDisconnectAlert.set(true); - } - case INVALID_DATA -> laserCanInvalidMeasure.set(true); - default -> { - if (Constants.ALLOW_ASSERTS) throw e; - else System.err.println("FIX NOW: Unhandled SensorRuntimeException: " + e.getMessage()); - } - } - } - - inputs.motorConnected = - BaseStatusSignal.refreshAll( - position, velocity, appliedVoltage, supplyCurrent, torqueCurrent, tempCelsius) - .isOK(); - inputs.positionRads = Units.rotationsToRadians(position.getValueAsDouble()) / reduction; - inputs.velocityRadsPerSec = Units.rotationsToRadians(velocity.getValueAsDouble()) / reduction; - inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); - inputs.supplyCurrentAmps = supplyCurrent.getValueAsDouble(); - inputs.torqueCurrentAmps = torqueCurrent.getValueAsDouble(); - inputs.tempCelsius = tempCelsius.getValueAsDouble(); - } - - @Override - public void runVolts(double volts) { - talon.setControl(voltageOut.withOutput(volts)); - } - - @Override - public void setAngle(double rads) throws IllegalArgumentException { - if (rads < Constants.CLIMBER_MIN_RADS || rads > Constants.CLIMBER_MAX_RADS) { - String message = - "Cannot set climber angle to " - + rads - + " rads. This value extends past the climber angle boundary."; - Logger.recordOutput("Climber/InvalidAngle", message); - throw new IllegalArgumentException(message); - } - rads = Math.min(Constants.CLIMBER_MIN_RADS, Math.max(rads, Constants.CLIMBER_MAX_RADS)); - talon.setControl(positionOut.withPosition(Radians.of(rads))); - } -} diff --git a/src/main/java/org/team5924/frc2025/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/org/team5924/frc2025/subsystems/elevator/ElevatorIOTalonFX.java index 9c0dd90..7463d30 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/org/team5924/frc2025/subsystems/elevator/ElevatorIOTalonFX.java @@ -16,10 +16,13 @@ package org.team5924.frc2025.subsystems.elevator; -import org.littletonrobotics.junction.Logger; -import org.team5924.frc2025.Constants; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Celsius; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; import static org.team5924.frc2025.Constants.ELEVATOR_LEFT_INVERSION; -import org.team5924.frc2025.util.LoggedTunableNumber; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; @@ -49,13 +52,6 @@ import com.ctre.phoenix6.signals.S1StateValue; import com.ctre.phoenix6.signals.S2CloseStateValue; import com.ctre.phoenix6.signals.S2StateValue; - -import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.Celsius; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -63,6 +59,9 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.DriverStation; +import org.littletonrobotics.junction.Logger; +import org.team5924.frc2025.Constants; +import org.team5924.frc2025.util.LoggedTunableNumber; /** TODO: Need to rezero elevator on min height. */