From ba2e2227a5b069b4ad4fc284285ba91fb3f559ec Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Wed, 4 Feb 2026 09:28:58 -0800 Subject: [PATCH 01/21] Add vendordep --- vendordeps/maple-sim.json | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 vendordeps/maple-sim.json diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json new file mode 100644 index 0000000..c73bea4 --- /dev/null +++ b/vendordeps/maple-sim.json @@ -0,0 +1,26 @@ +{ + "fileName": "maple-sim.json", + "name": "maplesim", + "version": "0.4.0-beta", + "frcYear": "2026", + "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", + "mavenUrls": [ + "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/repos/releases", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json", + "javaDependencies": [ + { + "groupId": "org.ironmaple", + "artifactId": "maplesim-java", + "version": "0.4.0-beta" + }, + { + "groupId": "org.dyn4j", + "artifactId": "dyn4j", + "version": "5.0.2" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} \ No newline at end of file From bf6d51f016e6be75b744562f2b63d253e9801365 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Wed, 4 Feb 2026 19:14:32 -0800 Subject: [PATCH 02/21] MapleSimgit add .! --- src/main/java/frc/robot/RobotContainer.java | 72 ++++++++- .../robot/subsystems/drive/GyroIOMaple.java | 25 +++ .../robot/subsystems/drive/ModuleIOMaple.java | 153 ++++++++++++++++++ .../java/frc/robot/util/MapleSimUtil.java | 73 +++++++++ src/main/java/frc/robot/util/PhoenixUtil.java | 19 +++ 5 files changed, 338 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/drive/GyroIOMaple.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOMaple.java create mode 100644 src/main/java/frc/robot/util/MapleSimUtil.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0e3cdaf..f71963e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,22 +6,36 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.FeetPerSecond; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; import java.util.Arrays; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.COTS; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnFly; +import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.net.WebServer; import edu.wpi.first.wpilibj.Filesystem; 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.button.RobotModeTriggers; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.Mode; +import frc.robot.Constants.RobotType; import frc.robot.commands.drive.DriveCommands; import frc.robot.generated.AlphaTunerConstants; import frc.robot.generated.BetaTunerConstants; @@ -29,8 +43,9 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; +import frc.robot.subsystems.drive.GyroIOMaple; +import frc.robot.subsystems.drive.ModuleIOMaple; import frc.robot.subsystems.drive.ModuleIOReplay; -import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants; @@ -39,6 +54,7 @@ import frc.robot.subsystems.vision.CameraIO; import frc.robot.subsystems.vision.CameraIOPhotonSim; import frc.robot.subsystems.vision.VisionConstants; +import frc.robot.util.MapleSimUtil; import frc.robot.util.Mechanism3d; public class RobotContainer { @@ -92,9 +108,34 @@ public RobotContainer() { case SIMBOT: // Sim robot, instantiate physics sim IO implementations + // Create and configure a drivetrain simulation configuration + DriveTrainSimulationConfig config = DriveTrainSimulationConfig.Default() + .withGyro(COTS.ofPigeon2()) + .withSwerveModule(COTS.ofMark4( + DCMotor.getKrakenX60(1), + DCMotor.getKrakenX60(1), + COTS.WHEELS.COLSONS.cof, + 2 + )) + .withTrackLengthTrackWidth( + Meters.of(Math.abs( + CompTunerConstants.FrontLeft.LocationX - + CompTunerConstants.BackLeft.LocationX + )), + Meters.of(Math.abs( + CompTunerConstants.FrontLeft.LocationX - + CompTunerConstants.FrontRight.LocationX + )) + ) + .withBumperSize(Inches.of(30.75), Inches.of(37.25)); + + // Add sim drivebase to simulation and where modules can get it. + // CALL THIS BEFORE CREATING THE DRIVEBASE!!! + MapleSimUtil.createSwerve(config, new Pose2d(2.0, 2.0, Rotation2d.kZero)); + drivebase_ = new Drive( - new GyroIO() {}, - ModuleIOSim::new, + new GyroIOMaple(), + ModuleIOMaple::new, CompTunerConstants.FrontLeft, CompTunerConstants.FrontRight, CompTunerConstants.BackLeft, @@ -105,7 +146,7 @@ public RobotContainer() { vision_ = new AprilTagVision( drivebase_::addVisionMeasurement, - new CameraIOPhotonSim("front", VisionConstants.frontTransform, drivebase_::getPose, true) + new CameraIOPhotonSim("front", VisionConstants.frontTransform, MapleSimUtil::getPosition, true) ); break; @@ -190,6 +231,29 @@ public RobotContainer() { Mechanism3d.measured.zero(); Mechanism3d.setpoints.zero(); + // Maple Sim + if (Constants.getRobot() == RobotType.SIMBOT) { + MapleSimUtil.start(); + + gamepad_.a().onTrue(Commands.runOnce(() -> { + var fuel = new RebuiltFuelOnFly( + MapleSimUtil.getPosition().getTranslation(), + new Translation2d(), // Initial Robot Position + MapleSimUtil.getFieldChassisSpeeds(), + MapleSimUtil.getPosition().getRotation(), + Meters.of(0.5), // Initial Height + MetersPerSecond.of(8), + Degrees.of(50) + ) + .withHitTargetCallBack(() -> System.out.println("Fuel Scored!")) + .withProjectileTrajectoryDisplayCallBack( + poses -> Logger.recordOutput("MapleSim/Trajectory", poses.toArray(Pose3d[]::new)) + ); + + SimulatedArena.getInstance().addGamePieceProjectile(fuel); + })); + } + // Choosers autoChooser_ = new LoggedDashboardChooser<>("Auto Choices"); autoChooser_.onChange(auto -> { diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOMaple.java b/src/main/java/frc/robot/subsystems/drive/GyroIOMaple.java new file mode 100644 index 0000000..9c0aedb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOMaple.java @@ -0,0 +1,25 @@ +package frc.robot.subsystems.drive; + +import static edu.wpi.first.units.Units.RadiansPerSecond; + +import org.ironmaple.simulation.drivesims.GyroSimulation; + +import frc.robot.util.MapleSimUtil; +import frc.robot.util.PhoenixUtil; + +public class GyroIOMaple implements GyroIO { + private final GyroSimulation sim; + + public GyroIOMaple() { + sim = MapleSimUtil.getGyroSimulation(); + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.connected = true; + inputs.yawPosition = sim.getGyroReading(); + inputs.yawVelocityRadPerSec = sim.getMeasuredAngularVelocity().in(RadiansPerSecond); + inputs.odometryYawPositions = sim.getCachedGyroReadings(); + inputs.odometryYawTimestamps = PhoenixUtil.getSimulationOdometryTimestamps(); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOMaple.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOMaple.java new file mode 100644 index 0000000..4b3ceef --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOMaple.java @@ -0,0 +1,153 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import static edu.wpi.first.units.Units.Amps; +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 java.util.Arrays; + +import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; +import org.ironmaple.simulation.motorsims.SimulatedMotorController; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import frc.robot.util.MapleSimUtil; +import frc.robot.util.PhoenixUtil; + +/** + * Physics sim implementation of module IO. The sim models are configured using a set of module constants from Phoenix. + * Simulation is always based on voltage control. + */ +public class ModuleIOMaple implements ModuleIO { + // TunerConstants doesn't support separate sim constants, so they are declared locally + private static final double DRIVE_KS = 0.03; + private static final double DRIVE_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation + private static final double DRIVE_KV = 1.0 / Units.rotationsToRadians(1.0 / DRIVE_KV_ROT); + + private final SwerveModuleSimulation moduleSimulation; + private final SimulatedMotorController.GenericMotorController driveMotor; + private final SimulatedMotorController.GenericMotorController turnMotor; + + private boolean driveClosedLoop = false; + private boolean turnClosedLoop = false; + private final PIDController driveController; + private final PIDController turnController; + private double driveFFVolts = 0.0; + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; + + private static int moduleIndex = 0; + + public ModuleIOMaple( + SwerveModuleConstants constants, + CANBus bus + ) { + moduleSimulation = MapleSimUtil.getModuleSimulations()[moduleIndex]; + moduleIndex++; + + this.driveMotor = moduleSimulation + .useGenericMotorControllerForDrive() + .withCurrentLimit(Amps.of(constants.SlipCurrent)); + this.turnMotor = moduleSimulation.useGenericControllerForSteer().withCurrentLimit(Amps.of(20)); + + this.driveController = new PIDController(0.05, 0.0, 0.0); + this.turnController = new PIDController(8.0, 0.0, 0.0); + + // Enable wrapping for turn PID + turnController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + // Run closed-loop control + if (driveClosedLoop) { + driveAppliedVolts = driveFFVolts + + driveController.calculate( + moduleSimulation.getDriveWheelFinalSpeed().in(RadiansPerSecond)); + } else { + driveController.reset(); + } + if (turnClosedLoop) { + turnAppliedVolts = turnController.calculate( + moduleSimulation.getSteerAbsoluteFacing().getRadians()); + } else { + turnController.reset(); + } + + // Update simulation state + driveMotor.requestVoltage(Volts.of(driveAppliedVolts)); + turnMotor.requestVoltage(Volts.of(turnAppliedVolts)); + + // Update drive inputs + inputs.driveConnected = true; + inputs.drivePositionRad = moduleSimulation.getDriveWheelFinalPosition().in(Radians); + inputs.driveVelocityRadPerSec = + moduleSimulation.getDriveWheelFinalSpeed().in(RadiansPerSecond); + inputs.driveAppliedVolts = driveAppliedVolts; + inputs.driveCurrentAmps = + Math.abs(moduleSimulation.getDriveMotorStatorCurrent().in(Amps)); + + // Update turn inputs + inputs.turnConnected = true; + inputs.turnEncoderConnected = true; + inputs.turnAbsolutePosition = moduleSimulation.getSteerAbsoluteFacing(); + inputs.turnPosition = moduleSimulation.getSteerAbsoluteFacing(); + inputs.turnVelocityRadPerSec = + moduleSimulation.getSteerAbsoluteEncoderSpeed().in(RadiansPerSecond); + inputs.turnAppliedVolts = turnAppliedVolts; + inputs.turnCurrentAmps = + Math.abs(moduleSimulation.getSteerMotorStatorCurrent().in(Amps)); + + // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't matter) + inputs.odometryTimestamps = PhoenixUtil.getSimulationOdometryTimestamps(); + inputs.odometryDrivePositionsRad = Arrays.stream(moduleSimulation.getCachedDriveWheelFinalPositions()) + .mapToDouble(angle -> angle.in(Radians)) + .toArray(); + inputs.odometryTurnPositions = moduleSimulation.getCachedSteerAbsolutePositions(); + } + + @Override + public void setDriveOpenLoop(double output) { + driveClosedLoop = false; + driveAppliedVolts = output; + } + + @Override + public void setTurnOpenLoop(double output) { + turnClosedLoop = false; + turnAppliedVolts = output; + } + + @Override + public void setDriveVelocity(double velocityRadPerSec) { + driveClosedLoop = true; + driveFFVolts = DRIVE_KS * Math.signum(velocityRadPerSec) + DRIVE_KV * velocityRadPerSec; + driveController.setSetpoint(velocityRadPerSec); + } + + @Override + public void setTurnPosition(Rotation2d rotation) { + turnClosedLoop = true; + turnController.setSetpoint(rotation.getRadians()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/MapleSimUtil.java b/src/main/java/frc/robot/util/MapleSimUtil.java new file mode 100644 index 0000000..11b9e73 --- /dev/null +++ b/src/main/java/frc/robot/util/MapleSimUtil.java @@ -0,0 +1,73 @@ +package frc.robot.util; + +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.GyroSimulation; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.simulation.seasonspecific.rebuilt2026.Arena2026Rebuilt; +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; + +public class MapleSimUtil { + private static SwerveDriveSimulation drivebaseSimulation; + + private static final Command run = + Commands.run(MapleSimUtil::periodic) + .ignoringDisable(true) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); + + /** Starts the MapleSim simulation, kicks off a periodic method. */ + public static void start() { + var rebuilt = (Arena2026Rebuilt) SimulatedArena.getInstance(); + rebuilt.setEfficiencyMode(false); + rebuilt.resetFieldForAuto(); + + CommandScheduler.getInstance().schedule(run); + } + + private static void periodic() { + var arena = SimulatedArena.getInstance(); + + arena.simulationPeriodic(); + + var fuel = arena.getGamePiecesArrayByType("Fuel"); + + Logger.recordOutput("MapleSim/Gamepieces", fuel); + Logger.recordOutput("MapleSim/Pose", getPosition()); + Logger.recordOutput("MapleSim/Score/Red", arena.getScore(Alliance.Red)); + Logger.recordOutput("MapleSim/Score/Blue", arena.getScore(Alliance.Blue)); + } + + public static void createSwerve(DriveTrainSimulationConfig config, Pose2d initialPose) { + drivebaseSimulation = new SwerveDriveSimulation(config, initialPose); + SimulatedArena.getInstance().addDriveTrainSimulation(drivebaseSimulation); + } + + public static GyroSimulation getGyroSimulation() { + if (drivebaseSimulation == null) throw new IllegalStateException("Drivebase sim is not setup yet!"); + return drivebaseSimulation.getGyroSimulation(); + } + + public static SwerveModuleSimulation[] getModuleSimulations() { + if (drivebaseSimulation == null) throw new IllegalStateException("Drivebase sim is not setup yet!"); + return drivebaseSimulation.getModules(); + } + + public static Pose2d getPosition() { + if (drivebaseSimulation == null) throw new IllegalStateException("Drivebase sim is not setup yet!"); + return drivebaseSimulation.getSimulatedDriveTrainPose(); + } + + public static ChassisSpeeds getFieldChassisSpeeds() { + if (drivebaseSimulation == null) throw new IllegalStateException("Drivebase sim is not setup yet!"); + return drivebaseSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(); + } + } diff --git a/src/main/java/frc/robot/util/PhoenixUtil.java b/src/main/java/frc/robot/util/PhoenixUtil.java index 95c9d02..bc08e5e 100644 --- a/src/main/java/frc/robot/util/PhoenixUtil.java +++ b/src/main/java/frc/robot/util/PhoenixUtil.java @@ -14,8 +14,15 @@ package frc.robot.util; import com.ctre.phoenix6.StatusCode; + +import edu.wpi.first.wpilibj.Timer; + +import static edu.wpi.first.units.Units.Seconds; + import java.util.function.Supplier; +import org.ironmaple.simulation.SimulatedArena; + public class PhoenixUtil { /** Attempts to run the command until no error is produced. */ public static void tryUntilOk(int maxAttempts, Supplier command) { @@ -24,4 +31,16 @@ public static void tryUntilOk(int maxAttempts, Supplier command) { if (error.isOK()) break; } } + + public static double[] getSimulationOdometryTimestamps() { + final double[] odometryTimeStamps = new double[SimulatedArena.getSimulationSubTicksIn1Period()]; + + for (int i = 0; i < odometryTimeStamps.length; i++) { + odometryTimeStamps[i] = Timer.getFPGATimestamp() + - 0.02 + + i * SimulatedArena.getSimulationDt().in(Seconds); + } + + return odometryTimeStamps; + } } From 05de7376e5751c158224886fd64b6954f3eaea9e Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Wed, 4 Feb 2026 19:21:39 -0800 Subject: [PATCH 03/21] Add comments --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 942d54b..f1af082 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -64,7 +64,7 @@ public static enum RobotType { BETA, /** The Sim Bot */ - SIMBOT, + SIMBOT } // This is only a fallback! This will not change the robot type. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f71963e..4e06597 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -130,7 +130,7 @@ public RobotContainer() { .withBumperSize(Inches.of(30.75), Inches.of(37.25)); // Add sim drivebase to simulation and where modules can get it. - // CALL THIS BEFORE CREATING THE DRIVEBASE!!! + // CALL THIS BEFORE CREATING THE DRIVEBASE! MapleSimUtil.createSwerve(config, new Pose2d(2.0, 2.0, Rotation2d.kZero)); drivebase_ = new Drive( From 042ba35d311252d0e275330fad1650cf9ca0990a Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Wed, 4 Feb 2026 20:19:49 -0800 Subject: [PATCH 04/21] Tune intake and make it simulatable --- src/main/java/frc/robot/RobotContainer.java | 50 ++++--------------- .../subsystems/intake/IntakeConstants.java | 23 ++++----- .../robot/subsystems/intake/IntakeIOSim.java | 11 ++-- .../subsystems/intake/IntakeSubsystem.java | 28 +++++------ 4 files changed, 40 insertions(+), 72 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c00dfe6..0e78a4d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ import edu.wpi.first.net.WebServer; import edu.wpi.first.wpilibj.Filesystem; 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.button.RobotModeTriggers; import frc.robot.Constants.DriveConstants; @@ -32,6 +33,9 @@ import frc.robot.subsystems.drive.ModuleIOReplay; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.subsystems.intake.IntakeIO; +import frc.robot.subsystems.intake.IntakeIOSim; +import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.shooter.ShooterIO; @@ -39,12 +43,6 @@ import frc.robot.subsystems.vision.CameraIO; import frc.robot.subsystems.vision.CameraIOPhotonSim; import frc.robot.subsystems.vision.VisionConstants; -import frc.robot.subsystems.intake.IntakeSubsystem; -import frc.robot.subsystems.intake.IntakeIO; -import frc.robot.subsystems.intake.IntakeIOSim; -import frc.robot.subsystems.intake.IntakeIOTalonFX; -import frc.robot.subsystems.intake.IntakeConstants; -import static edu.wpi.first.units.Units.Volts; import frc.robot.util.Mechanism3d; public class RobotContainer { @@ -221,41 +219,11 @@ public RobotContainer() { // Bind robot actions to commands here. private void configureBindings() { //Testing out each of the commands in the simulator - gamepad_.a().whileTrue( - intake_.setRollerVoltageCommand(IntakeConstants.rollerCollectVoltage) - ); - - gamepad_.b().whileTrue( - intake_.setPivotAngleCommand(IntakeConstants.pivotTargetAngle) - ); - - gamepad_.x().whileTrue( - intake_.deployIntakeCommand() - ); - - gamepad_.y().whileTrue( - intake_.stowIntakeCommand() - ); - - gamepad_.leftBumper().whileTrue( - intake_.stopRollerCommand() - ); - - gamepad_.rightBumper().whileTrue( - intake_.setRollerVelocityCommand(IntakeConstants.rollerMaxVelocity) - ); - - gamepad_.back().whileTrue( - intake_.setPivotVoltageCommand(IntakeConstants.pivotVoltage) - ); - - gamepad_.rightTrigger().whileTrue( - intake_.intakeDeployCommand() - ); - - gamepad_.leftTrigger().whileTrue( - intake_.stopStowCommand() - ); + gamepad_.x().onTrue(Commands.either( + intake_.deployIntakeCommand(), + intake_.stowIntakeCommand(), + intake_::isIntakeStowed + )); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 2a749ba..175ab91 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -22,9 +22,8 @@ public final class IntakeConstants { public static final int currentLimit= 40; public static final Time currentLimitTime= Seconds.of(1); //Temporary - public static final double pivotKP= 0.5; //later for turning - public static final double pivotKD= 0.05; //later for turning + public static final double pivotKD= 0.04; //later for turning public static final double pivotKV= 0; //later for turning public static final double pivotKI= 0; //later for turning public static final double pivotKA= 0; //later for turning @@ -35,15 +34,15 @@ public final class IntakeConstants { public static final double rollerKD= 0; //change later public static final double rollerKV= 0; //change later public static final double rollerKI= 0; //change later - public static final double rollerKA=0; //change later + public static final double rollerKA = 0; //change later public static final double rollerKS= 0; //change later public static final double rollerKG= 0; //change later - public static final Angle stowedAngle= Degrees.of(-20); //Temporary angle - public static final Angle deployedAngle= Degrees.of(-35); //Temporary angle + public static final Angle stowedAngle= Degrees.of(0); //Temporary angle + public static final Angle deployedAngle= Degrees.of(125); //Temporary angle public static final Angle pivotMinAngle= Degrees.of(-15); //Temporary angle - public static final Angle pivotMaxAngle= Degrees.of(-40); //Temporary angle - public static final Angle pivotTargetAngle= Degrees.of(-25); //This variable can be changed if we want to run a command to tell intake to go to a specific position + public static final Angle pivotMaxAngle= Degrees.of(130); //Temporary angle + public static final Angle pivotTargetAngle = Degrees.of(-25); //This variable can be changed if we want to run a command to tell intake to go to a specific position public static final Voltage pivotVoltage= Volts.of(2); //Temporary if we want to set the pivot voltage to a specifc value at all public static final AngularVelocity pivotCruiseVelocity= DegreesPerSecond.of(90); //Temporary speed @@ -53,10 +52,10 @@ public final class IntakeConstants { public static final AngularVelocity rollerMaxVelocity= DegreesPerSecond.of(360); //Temporary speed to be changed as needed public static final Voltage rollerCollectVoltage= Volts.of(6); //Temporary voltage - public static final double pivotDegreeTolerance= 2; //Tolerance to compare current angle to target + public static final Angle pivotTolerance = Degrees.of(5); //Tolerance to compare current angle to target - public static final double pivotGearRatio= 0; //Unsure yet, will need it for the simulator - public static final double rollerGearRatio= 0; //Unsure yet, will need it for the simulator - public static final MomentOfInertia PIVOT_MOMENTOFINERTIA= KilogramSquareMeters.of(0.01); - public static final MomentOfInertia ROLLER_MOMENTOFINERTIA= KilogramSquareMeters.of(0.01); + public static final double motorToPivotGearRatio = 2.0; //Unsure yet, will need it for the simulator + public static final double rollerGearRatio = 1.0; //Unsure yet, will need it for the simulator + public static final MomentOfInertia PIVOT_MOMENTOFINERTIA = KilogramSquareMeters.of(0.01); + public static final MomentOfInertia ROLLER_MOMENTOFINERTIA = KilogramSquareMeters.of(0.01); } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index eef5d0f..57745b4 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -9,15 +9,16 @@ import edu.wpi.first.wpilibj.RobotController; import static edu.wpi.first.units.Units.KilogramSquareMeters; -public class IntakeIOSim extends IntakeIOTalonFX{ +public class IntakeIOSim extends IntakeIOTalonFX { public final DCMotorSim pivotMotorSim; public final DCMotorSim rollerMotorSim; public IntakeIOSim() { - super(CompTunerConstants.kCANBus, CompTunerConstants.kCANBus); + super(CompTunerConstants.kCANBus, CompTunerConstants.kCANBus); // This needs to be changed + pivotMotorSim= new DCMotorSim( LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX60Foc(1), IntakeConstants.PIVOT_MOMENTOFINERTIA.in(KilogramSquareMeters), IntakeConstants.pivotGearRatio + DCMotor.getKrakenX60Foc(1), IntakeConstants.PIVOT_MOMENTOFINERTIA.in(KilogramSquareMeters), IntakeConstants.motorToPivotGearRatio ), DCMotor.getKrakenX60Foc(1) //Not sure how many motors ); @@ -41,8 +42,8 @@ public void updateInputs(IntakeIOInputsAutoLogged inputs){ pivotMotorSim.setInputVoltage(pivotMotorSimState.getMotorVoltage()); pivotMotorSim.update(0.020); //20 millisecond robot sim loop - pivotMotorSimState.setRawRotorPosition(pivotMotorSim.getAngularPosition().times(IntakeConstants.pivotGearRatio)); - pivotMotorSimState.setRotorVelocity(pivotMotorSim.getAngularVelocity().times(IntakeConstants.pivotGearRatio)); + pivotMotorSimState.setRawRotorPosition(pivotMotorSim.getAngularPosition().times(IntakeConstants.motorToPivotGearRatio)); + pivotMotorSimState.setRotorVelocity(pivotMotorSim.getAngularVelocity().times(IntakeConstants.motorToPivotGearRatio)); rollerMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 64aee11..21c83f6 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.util.Mechanism3d; import org.littletonrobotics.junction.Logger; import edu.wpi.first.units.measure.Voltage; @@ -19,6 +20,8 @@ public class IntakeSubsystem extends SubsystemBase { private final Angle pivotDeployedAngle = IntakeConstants.deployedAngle; private final Angle pivotStowedAngle = IntakeConstants.stowedAngle; + private Angle setpointAngle = pivotStowedAngle; + public IntakeSubsystem(IntakeIO io) { this.io = io; Logger.processInputs("Intake", inputs); @@ -28,6 +31,11 @@ public IntakeSubsystem(IntakeIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Intake", inputs); + + Logger.recordOutput("Intake/Setpoint", setpointAngle); + + Mechanism3d.measured.setIntake(inputs.PivotAngle); + Mechanism3d.setpoints.setIntake(setpointAngle); } //Intake control methods @@ -36,11 +44,12 @@ public void setRollerVoltage(Voltage volts) { } public void setPivotAngle(Angle angle) { + setpointAngle = angle; io.setPivotAngle(angle); } public void stopRoller(){ - io.setRollerVoltage(Volts.of(0)); + io.stopRoller(); } public void stowIntake(){ @@ -63,25 +72,16 @@ public Angle getPivotAngle(){ return inputs.PivotAngle; } - public boolean isIntakeDeployed(){ - // Compare pivot angle to deployed angle with a small tolerance (degrees) - double currentDeg = getPivotAngle().in(Degrees); - double targetDeg = pivotDeployedAngle.in(Degrees); - return Math.abs(currentDeg - targetDeg) <= IntakeConstants.pivotDegreeTolerance; + public boolean isIntakeDeployed() { + return isPivotAtAngle(pivotDeployedAngle); } public boolean isIntakeStowed(){ - // Compare pivot angle to stowed angle with a small tolerance (degrees) - double currentDeg = getPivotAngle().in(Degrees); - double targetDeg = pivotStowedAngle.in(Degrees); - return Math.abs(currentDeg - targetDeg) <= IntakeConstants.pivotDegreeTolerance; + return isPivotAtAngle(pivotStowedAngle); } public boolean isPivotAtAngle(Angle angle){ - // Compare pivot angle to target angle with a small tolerance (degrees) - double currentDeg = getPivotAngle().in(Degrees); - double targetDeg = angle.in(Degrees); - return Math.abs(currentDeg - targetDeg) <= IntakeConstants.pivotDegreeTolerance; + return inputs.PivotAngle.isNear(angle, IntakeConstants.pivotTolerance); } ///////////// From 0e1cce40f4ebef7114a4aada235f6302fa5f6dfd Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Wed, 4 Feb 2026 21:02:49 -0800 Subject: [PATCH 05/21] Make simulation pass 1 --- src/main/java/frc/robot/Constants.java | 2 + src/main/java/frc/robot/RobotContainer.java | 2 + .../subsystems/intake/IntakeSubsystem.java | 12 ++++ .../java/frc/robot/util/MapleSimUtil.java | 69 +++++++++++++++---- 4 files changed, 70 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f1af082..654ca44 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,6 +33,8 @@ public final class Constants { // Sets the currently running robot. private static final RobotType robotType = RobotType.SIMBOT; + public static final boolean spawnLessFuelInSim = true; + public static class DriveConstants { public static final double slowModeJoystickMultiplier = 0.4; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e0df1a4..7c25acb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -260,6 +260,8 @@ public RobotContainer() { poses -> Logger.recordOutput("MapleSim/Trajectory", poses.toArray(Pose3d[]::new)) ); + MapleSimUtil.loseGamepiece(); + SimulatedArena.getInstance().addGamePieceProjectile(fuel); })); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 21c83f6..3be7451 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -4,6 +4,10 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; +import frc.robot.Constants.Mode; +import frc.robot.Constants.RobotType; +import frc.robot.util.MapleSimUtil; import frc.robot.util.Mechanism3d; import org.littletonrobotics.junction.Logger; @@ -25,6 +29,10 @@ public class IntakeSubsystem extends SubsystemBase { public IntakeSubsystem(IntakeIO io) { this.io = io; Logger.processInputs("Intake", inputs); + + if (Constants.getMode() == Mode.SIM) { + MapleSimUtil.createIntake(); + } } @Override @@ -36,6 +44,10 @@ public void periodic() { Mechanism3d.measured.setIntake(inputs.PivotAngle); Mechanism3d.setpoints.setIntake(setpointAngle); + + if (Constants.getMode() == Mode.SIM) { + MapleSimUtil.setIntakeRunning(isIntakeDeployed()); + } } //Intake control methods diff --git a/src/main/java/frc/robot/util/MapleSimUtil.java b/src/main/java/frc/robot/util/MapleSimUtil.java index 11b9e73..546c3be 100644 --- a/src/main/java/frc/robot/util/MapleSimUtil.java +++ b/src/main/java/frc/robot/util/MapleSimUtil.java @@ -1,6 +1,11 @@ package frc.robot.util; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; + +import org.ironmaple.simulation.IntakeSimulation; import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.IntakeSimulation.IntakeSide; import org.ironmaple.simulation.drivesims.GyroSimulation; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; @@ -13,12 +18,15 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; +import frc.robot.Constants; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; public class MapleSimUtil { private static SwerveDriveSimulation drivebaseSimulation; + private static IntakeSimulation intakeSimulation; + private static final Command run = Commands.run(MapleSimUtil::periodic) .ignoringDisable(true) @@ -27,14 +35,14 @@ public class MapleSimUtil { /** Starts the MapleSim simulation, kicks off a periodic method. */ public static void start() { var rebuilt = (Arena2026Rebuilt) SimulatedArena.getInstance(); - rebuilt.setEfficiencyMode(false); + rebuilt.setEfficiencyMode(Constants.spawnLessFuelInSim); rebuilt.resetFieldForAuto(); CommandScheduler.getInstance().schedule(run); } private static void periodic() { - var arena = SimulatedArena.getInstance(); + var arena = (Arena2026Rebuilt) SimulatedArena.getInstance(); arena.simulationPeriodic(); @@ -44,30 +52,61 @@ private static void periodic() { Logger.recordOutput("MapleSim/Pose", getPosition()); Logger.recordOutput("MapleSim/Score/Red", arena.getScore(Alliance.Red)); Logger.recordOutput("MapleSim/Score/Blue", arena.getScore(Alliance.Blue)); + Logger.recordOutput("MapleSim/FuelRemaining", getRemainingGamepieces()); + Logger.recordOutput("MapleSim/Intaking", intakeSimulation.isRunning()); + Logger.recordOutput("MapleSim/ActiveHub", arena.isActive(true) ? Alliance.Blue : Alliance.Red); } - public static void createSwerve(DriveTrainSimulationConfig config, Pose2d initialPose) { + public static SwerveDriveSimulation createSwerve(DriveTrainSimulationConfig config, Pose2d initialPose) { drivebaseSimulation = new SwerveDriveSimulation(config, initialPose); SimulatedArena.getInstance().addDriveTrainSimulation(drivebaseSimulation); - } - - public static GyroSimulation getGyroSimulation() { - if (drivebaseSimulation == null) throw new IllegalStateException("Drivebase sim is not setup yet!"); - return drivebaseSimulation.getGyroSimulation(); - } - - public static SwerveModuleSimulation[] getModuleSimulations() { - if (drivebaseSimulation == null) throw new IllegalStateException("Drivebase sim is not setup yet!"); - return drivebaseSimulation.getModules(); + return drivebaseSimulation; } public static Pose2d getPosition() { - if (drivebaseSimulation == null) throw new IllegalStateException("Drivebase sim is not setup yet!"); return drivebaseSimulation.getSimulatedDriveTrainPose(); } public static ChassisSpeeds getFieldChassisSpeeds() { - if (drivebaseSimulation == null) throw new IllegalStateException("Drivebase sim is not setup yet!"); return drivebaseSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(); } + + public static IntakeSimulation createIntake() { + if (drivebaseSimulation == null) throw new IllegalStateException("Intake cannot be created before swerve is!"); + + intakeSimulation = IntakeSimulation.OverTheBumperIntake( + "Fuel", + drivebaseSimulation, + Inches.of(29.5), + Inches.of(8.125), + IntakeSide.FRONT, + 80 + ); + + return intakeSimulation; + } + + public static int getRemainingGamepieces() { + return intakeSimulation.getGamePiecesAmount(); + } + + public static void setIntakeRunning(boolean running) { + if (running) { + intakeSimulation.startIntake(); + } else { + intakeSimulation.stopIntake(); + } + } + + public static void loseGamepiece() { + intakeSimulation.obtainGamePieceFromIntake(); + } + + public static GyroSimulation getGyroSimulation() { + return drivebaseSimulation.getGyroSimulation(); + } + + public static SwerveModuleSimulation[] getModuleSimulations() { + return drivebaseSimulation.getModules(); + } } From e63cd5e9f44bf0c39e2d2ddf59ac2447d329d8f5 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Thu, 5 Feb 2026 20:29:51 -0800 Subject: [PATCH 06/21] Integrate Shooter pass 1 --- src/main/java/frc/robot/RobotContainer.java | 18 +- .../subsystems/intake/IntakeSubsystem.java | 163 +++++++----------- .../frc/robot/subsystems/shooter/Shooter.java | 92 ++++++---- .../subsystems/shooter/ShooterConstants.java | 5 +- .../robot/subsystems/shooter/ShooterIO.java | 2 + .../subsystems/shooter/ShooterIOTalonFX.java | 36 ++-- .../java/frc/robot/util/MapleSimUtil.java | 41 +++-- 7 files changed, 184 insertions(+), 173 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9f0324d..f1d05f8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -140,6 +140,7 @@ public RobotContainer() { // Add sim drivebase to simulation and where modules can get it. // CALL THIS BEFORE CREATING THE DRIVEBASE! MapleSimUtil.createSwerve(config, new Pose2d(2.0, 2.0, Rotation2d.kZero)); + MapleSimUtil.createIntake(); drivebase_ = new Drive( new GyroIOMaple(), @@ -310,12 +311,21 @@ public RobotContainer() { private void configureBindings() { gamepad_.a().onTrue(thriftyClimb_.toggle()); //Testing out each of the commands in the simulator - gamepad_.x().onTrue(Commands.either( - intake_.deployIntakeCommand(), - intake_.stowIntakeCommand(), + gamepad_.start().onTrue(Commands.either( + intake_.deployCmd(), + intake_.stowCmd(), intake_::isIntakeStowed )); + // While the left trigger is held, we will run the intake. If the intake is stowed, it will also deploy it. + gamepad_.leftTrigger().whileTrue( + intake_.runIntakeCmd().beforeStarting( + intake_.deployCmd().onlyIf(intake_::isIntakeStowed) + ) + ); + + // While the right trigger is held, we will shoot into the hub. + gamepad_.rightTrigger().whileTrue(shooter_.shootCmd()); } private void configureDriveBindings() { @@ -374,7 +384,7 @@ private void configureTestModeBindings() { LoggedNetworkNumber hoodAngle = new LoggedNetworkNumber("Tuning/Shooter/TargetHoodAngle", ShooterConstants.SoftwareLimits.hoodMinAngle); gamepad_.a().and(RobotModeTriggers.test()).toggleOnTrue( - shooter_.runDynamicSetpoint(() -> RotationsPerSecond.of(shooterVelocity.get()), () -> Degrees.of(hoodAngle.get())) + shooter_.runDynamicSetpoints(() -> RotationsPerSecond.of(shooterVelocity.get()), () -> Degrees.of(hoodAngle.get())) ); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 3be7451..52bde3e 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -1,23 +1,23 @@ package frc.robot.subsystems.intake; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.Mode; -import frc.robot.Constants.RobotType; import frc.robot.util.MapleSimUtil; import frc.robot.util.Mechanism3d; -import org.littletonrobotics.junction.Logger; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Time; - -import static edu.wpi.first.units.Units.*; - public class IntakeSubsystem extends SubsystemBase { private final IntakeIO io; private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); @@ -28,11 +28,6 @@ public class IntakeSubsystem extends SubsystemBase { public IntakeSubsystem(IntakeIO io) { this.io = io; - Logger.processInputs("Intake", inputs); - - if (Constants.getMode() == Mode.SIM) { - MapleSimUtil.createIntake(); - } } @Override @@ -40,46 +35,54 @@ public void periodic() { io.updateInputs(inputs); Logger.processInputs("Intake", inputs); - Logger.recordOutput("Intake/Setpoint", setpointAngle); + Logger.recordOutput("Intake/PivotSetpoint", setpointAngle); Mechanism3d.measured.setIntake(inputs.PivotAngle); Mechanism3d.setpoints.setIntake(setpointAngle); if (Constants.getMode() == Mode.SIM) { - MapleSimUtil.setIntakeRunning(isIntakeDeployed()); + MapleSimUtil.setIntakeRunning(isIntakeDeployed() && inputs.RollerAngularVelocity.gt(RadiansPerSecond.zero())); } } //Intake control methods - public void setRollerVoltage(Voltage volts) { + private void setRollerVoltage(Voltage volts) { io.setRollerVoltage(volts); } - public void setPivotAngle(Angle angle) { + private void setPivotAngle(Angle angle) { setpointAngle = angle; io.setPivotAngle(angle); } - public void stopRoller(){ + /** + * Runs the roller. + */ + private void startIntaking() { + io.setRollerVoltage(IntakeConstants.rollerCollectVoltage); + } + + /** + * Stops the roller. + */ + private void stopIntaking() { io.stopRoller(); } - public void stowIntake(){ + /** + * Stows the intake and hopper. + */ + private void stow() { setPivotAngle(IntakeConstants.stowedAngle); } - public void deployIntake(){ + /** + * Deploys the intake and hopper. + */ + private void deploy() { setPivotAngle(IntakeConstants.deployedAngle); } - public void setRollerVelocity(AngularVelocity velocity) { - io.setRollerVelocity(velocity); - } - - public void setPivotVoltage(Voltage voltage) { - io.setPivotVoltage(voltage); - } - public Angle getPivotAngle(){ return inputs.PivotAngle; } @@ -100,82 +103,34 @@ public boolean isPivotAtAngle(Angle angle){ ///Commands// ///////////// - ////////////////////////////////////////////////////// - /// First stow the intake, and then wait until it is stowed - ////////////////////////////////////////////////////// - public Command stowIntakeCommand() { - return Commands.runOnce(() -> stowIntake()) - .andThen(Commands.waitUntil(() -> isIntakeStowed()) - .withTimeout(2)).withName("Stow Intake"); - } - - ////////////////////////////////////////////////////// - /// First deploy the intake, and then wait until it is deployed - ////////////////////////////////////////////////////// - public Command deployIntakeCommand() { - return Commands.runOnce(() -> deployIntake()) - .andThen(Commands.waitUntil(() -> isIntakeDeployed()) - .withTimeout(2)).withName("Deploy Intake"); - } - - ///////////////////////// - /// Stop the rollers once - ///////////////////////// - public Command stopRollerCommand() { - return Commands.runOnce(() -> stopRoller()) - .withName("Stop Roller"); - } - - //////////////////////////////////////////////////////// - /// Continously set the roller voltage until interrupted - //////////////////////////////////////////////////////// - public Command setRollerVoltageCommand(Voltage volts) { - return Commands.run(() -> setRollerVoltage(volts)) - .withName("Set Roller Voltage"); - } - - ////////////////////////////////////////////////////////////////// - /// Continously move the rollers at set velocity until interrupted - ////////////////////////////////////////////////////////////////// - public Command setRollerVelocityCommand(AngularVelocity velocity) { - return Commands.run(() -> setRollerVelocity(velocity)) - .withName("Set Roller Velocity"); + /** + * Command that stows the intake and ends when it gets there. + * @return + */ + public Command stowCmd() { + return Commands.runOnce(() -> stow()) + .andThen(Commands.waitUntil(() -> isIntakeStowed()) + .withTimeout(2)).withName("Stow Intake"); + } + + /** + * Command that deploys the intake and ends when it gets there. + * @return + */ + public Command deployCmd() { + return runOnce(() -> deploy()) + .andThen(Commands.waitUntil(() -> isIntakeDeployed()) + .withTimeout(2)).withName("Deploy Intake"); + } + + /** + * Command that runs the intake until the command ends. + * @return The command + */ + public Command runIntakeCmd() { + return startEnd(this::startIntaking, this::stopIntaking); } - //////////////////////////////////////////////////////////////// - /// Continously move the pivot at set velocity until interrupted - //////////////////////////////////////////////////////////////// - public Command setPivotVoltageCommand(Voltage voltage) { - return Commands.run(() -> setPivotVoltage(voltage)) - .withName("Set Pivot Voltage"); - } - - ///////////////////////////////////////////////////////////// - /// First set the angle target, then wait until it is reached - ///////////////////////////////////////////////////////////// - public Command setPivotAngleCommand(Angle angle) { - return Commands.runOnce(() ->setPivotAngle(angle)) - .andThen(Commands.waitUntil(() -> isPivotAtAngle(angle)) - .withTimeout(2)).withName("Set Pivot Angle"); - } - - //////////////////////////////// - ///Deploy and intake in parallel - //////////////////////////////// - public Command intakeDeployCommand(){ - return Commands.runOnce(() -> setRollerVoltage(IntakeConstants.rollerCollectVoltage)) - .andThen(Commands.runOnce(()->deployIntake() )); - } - - //////////////////////////////////////// - ///Stow and stop the rollers in parallel - //////////////////////////////////////// - public Command stopStowCommand(){ - return Commands.runOnce(() -> stopRollerCommand()) - .andThen(Commands.runOnce(()-> stowIntakeCommand())); - } - - //////////////////////////// /// Sys ID Routine creation/ /// //////////////////////// @@ -191,7 +146,7 @@ public final SysIdRoutine pivotSysIdRoutine(){ (state) -> Logger.recordOutput("state", state.toString()) //Logging the state of the routine ), new SysIdRoutine.Mechanism( - (Voltage voltage)-> setPivotVoltage(voltage), + (Voltage voltage)-> io.setPivotVoltage(voltage), null, this ) diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index f7b12c0..25c83c3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,7 +1,9 @@ package frc.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RPM; import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RevolutionsPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; @@ -19,81 +21,92 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; +import frc.robot.Constants.Mode; +import frc.robot.util.MapleSimUtil; +import frc.robot.util.Mechanism3d; public class Shooter extends SubsystemBase { private final ShooterIO io_; - private final ShooterIOInputsAutoLogged inputs_; - private AngularVelocity shooterTarget; - private Angle hoodTarget; + private final ShooterIOInputsAutoLogged inputs_ = new ShooterIOInputsAutoLogged(); + + private AngularVelocity shooterTarget = RadiansPerSecond.zero(); + private Angle hoodTarget = Radians.zero(); public Shooter(ShooterIO io) { io_ = io; - inputs_ = new ShooterIOInputsAutoLogged(); - shooterTarget = RotationsPerSecond.zero(); - hoodTarget = Radians.zero(); } @Override public void periodic() { io_.updateInputs(inputs_); Logger.processInputs("Shooter", inputs_); + + Mechanism3d.measured.setHood(hoodTarget); + Mechanism3d.setpoints.setHood(hoodTarget); + + if (Constants.getMode() == Mode.SIM) { + MapleSimUtil.setShooterVelocity(inputs_.wheelVelocity); + MapleSimUtil.setHoodAngle(hoodTarget); + } + Logger.recordOutput("Shooter/VelocitySetPoint", shooterTarget); Logger.recordOutput("Shooter/HoodSetPoint", hoodTarget); } // Shooter Methods - public void setShooterVelocity(AngularVelocity vel) { + private void setShooterVelocity(AngularVelocity vel) { shooterTarget = vel; io_.setShooterVelocity(vel); } - public void setShooterVoltage(Voltage vol) { - shooterTarget = RotationsPerSecond.of(0.0); + private void setShooterVoltage(Voltage vol) { + shooterTarget = RotationsPerSecond.zero(); io_.setShooterVoltage(vol); } - public void stopShooter() { - shooterTarget = RotationsPerSecond.of(0.0); + private void stopShooter() { + shooterTarget = RotationsPerSecond.zero(); io_.stopShooter(); } public AngularVelocity getShooterVelocity() { - return inputs_.shooter1Velocity; - } - - public boolean isShooterReady() { - return Math.abs(inputs_.shooter1Velocity.in(RotationsPerSecond) - shooterTarget.in(RotationsPerSecond)) < ShooterConstants.shooterTolerance.in(RevolutionsPerSecond); + return inputs_.wheelVelocity; } public Voltage getShooterVoltage() { return inputs_.shooter1Voltage; } + + public boolean isShooterReady() { + return inputs_.wheelVelocity.isNear(shooterTarget, ShooterConstants.shooterTolerance); + } public Current getShooterCurrent() { return (inputs_.shooter1Current) - .plus(inputs_.shooter2Current) - .plus(inputs_.shooter3Current) - .plus(inputs_.shooter4Current); + .plus(inputs_.shooter2Current) + .plus(inputs_.shooter3Current) + .plus(inputs_.shooter4Current); } - public Command setVelocityCmd(AngularVelocity vel) { + public Command runToVelocityCmd(AngularVelocity vel) { return runOnce(() -> setShooterVelocity(vel)) - .andThen(Commands.waitUntil(() -> isShooterReady())).withName("Set Shooter Velocity"); + .andThen(Commands.waitUntil(this::isShooterReady)).withName("Set Shooter Velocity"); } - public Command stopCommand() { + public Command stopCmd() { return runOnce(() -> stopShooter()) - .andThen(Commands.waitUntil(() -> isShooterReady())).withName("Stop Shooter"); + .andThen(Commands.waitUntil(this::isShooterReady)).withName("Stop Shooter"); } - public Command setVoltageCmd(Voltage vol) { + public Command runVoltageCmd(Voltage vol) { return runOnce(() -> setShooterVoltage(vol)).withName("Set Shooter Voltage"); } // Hood Methods - public void setHoodAngle(Angle pos) { + private void setHoodAngle(Angle pos) { hoodTarget = pos; io_.setHoodPosition(pos); } @@ -103,19 +116,32 @@ public Command hoodToPosCmd(Angle pos) { } // Both - public void goToShooterPosition(AngularVelocity vel, Angle pos) { + private void setSetpoints(AngularVelocity vel, Angle pos) { shooterTarget = vel; - this.setShooterVelocity(shooterTarget); + setShooterVelocity(shooterTarget); hoodTarget = pos; - this.setHoodAngle(hoodTarget); + setHoodAngle(hoodTarget); + } + + /** + * Shoot balls into the hub until the command ends. + * @return + */ + public Command shootCmd() { + return startEnd(() -> { + io_.setShooterVelocity(RPM.of(1000)); + io_.setHoodPosition(Degrees.of(45)); + }, () -> { + io_.stopShooter(); + }); } - public Command runSetpointCmd(AngularVelocity vel, Angle pos) { - return runOnce(() -> goToShooterPosition(vel, pos)).andThen(Commands.waitUntil(this::isShooterReady)); + public Command runToSetpointsCmd(AngularVelocity vel, Angle pos) { + return runOnce(() -> setSetpoints(vel, pos)).andThen(Commands.waitUntil(this::isShooterReady)); } - public Command runDynamicSetpoint(Supplier vel, Supplier pos ) { - return run(() -> goToShooterPosition(vel.get(), pos.get())); + public Command runDynamicSetpoints(Supplier vel, Supplier pos ) { + return run(() -> setSetpoints(vel.get(), pos.get())); } // Sys ID diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index f3b8171..100dca1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -15,18 +15,17 @@ public class ShooterConstants { public static final int hoodCANID = 4; - public static final int currentLimit = 40; public static final Time currentLimitTime = Seconds.of(1); - public static final double gearRatio = 75/27; + public static final double gearRatio = 75.0 / 27.0; public static final AngularVelocity shooterTolerance = RotationsPerSecond.of(1.0); public class PID { // shooter - public static final double shooterkP = 0.0; + public static final double shooterkP = 0.5; public static final double shooterkI = 0.0; public static final double shooterkD = 0.0; public static final double shooterkV = 0.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 3073880..c72fca9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -40,6 +40,8 @@ public static class ShooterIOInputs { public Voltage hoodVoltage = Volts.zero(); public Current hoodCurrent = Amps.zero(); public AngularVelocity hoodVelocity = RadiansPerSecond.zero(); + + public AngularVelocity wheelVelocity = RadiansPerSecond.zero(); } public default void updateInputs(ShooterIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 99cd9ed..1b0b32a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -27,7 +27,6 @@ public class ShooterIOTalonFX implements ShooterIO { private Servo hood1; private Servo hood2; - private StatusSignalCollection signals; @@ -55,7 +54,7 @@ public ShooterIOTalonFX(CANBus shooterCANBus) { hood2 = new Servo(1); final TalonFXConfiguration shooterConfigs = new TalonFXConfiguration(); - final TalonFXConfiguration hoodConfigs = new TalonFXConfiguration(); + // final TalonFXConfiguration hoodConfigs = new TalonFXConfiguration(); // PID shooterConfigs.Slot0.kP = ShooterConstants.PID.shooterkP; @@ -67,20 +66,20 @@ public ShooterIOTalonFX(CANBus shooterCANBus) { shooterConfigs.Slot0.kS = ShooterConstants.PID.shooterkS; - hoodConfigs.Slot0.kP = ShooterConstants.PID.hoodkP; - hoodConfigs.Slot0.kD = ShooterConstants.PID.hoodkD; - hoodConfigs.Slot0.kI = ShooterConstants.PID.hoodkI; - hoodConfigs.Slot0.kV = ShooterConstants.PID.hoodkV; - hoodConfigs.Slot0.kA = ShooterConstants.PID.hoodkA; - hoodConfigs.Slot0.kG = ShooterConstants.PID.hoodkG; - hoodConfigs.Slot0.kS = ShooterConstants.PID.hoodkS; + // hoodConfigs.Slot0.kP = ShooterConstants.PID.hoodkP; + // hoodConfigs.Slot0.kD = ShooterConstants.PID.hoodkD; + // hoodConfigs.Slot0.kI = ShooterConstants.PID.hoodkI; + // hoodConfigs.Slot0.kV = ShooterConstants.PID.hoodkV; + // hoodConfigs.Slot0.kA = ShooterConstants.PID.hoodkA; + // hoodConfigs.Slot0.kG = ShooterConstants.PID.hoodkG; + // hoodConfigs.Slot0.kS = ShooterConstants.PID.hoodkS; // Software Limits - only hood - hoodConfigs.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - hoodConfigs.SoftwareLimitSwitch.ForwardSoftLimitThreshold = ShooterConstants.SoftwareLimits.hoodMaxAngle; - hoodConfigs.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - hoodConfigs.SoftwareLimitSwitch.ReverseSoftLimitThreshold = ShooterConstants.SoftwareLimits.hoodMinAngle; + // hoodConfigs.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; + // hoodConfigs.SoftwareLimitSwitch.ForwardSoftLimitThreshold = ShooterConstants.SoftwareLimits.hoodMaxAngle; + // hoodConfigs.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + // hoodConfigs.SoftwareLimitSwitch.ReverseSoftLimitThreshold = ShooterConstants.SoftwareLimits.hoodMinAngle; // Motion Magic Configurations shooterConfigs.MotionMagic.MotionMagicCruiseVelocity = ShooterConstants.MotionMagic.shooterkMaxVelocity; @@ -91,8 +90,8 @@ public ShooterIOTalonFX(CANBus shooterCANBus) { shooterConfigs.CurrentLimits.StatorCurrentLimit = ShooterConstants.currentLimit; shooterConfigs.CurrentLimits.StatorCurrentLimitEnable = true; - hoodConfigs.CurrentLimits.StatorCurrentLimit = ShooterConstants.currentLimit; - shooterConfigs.CurrentLimits.StatorCurrentLimitEnable = true; + // hoodConfigs.CurrentLimits.StatorCurrentLimit = ShooterConstants.currentLimit; + // hoodConfigs.CurrentLimits.StatorCurrentLimitEnable = true; // Similar to our checkError function tryUntilOk(5, () -> shooter1Motor.getConfigurator().apply(shooterConfigs, 0.25)); @@ -103,7 +102,6 @@ public ShooterIOTalonFX(CANBus shooterCANBus) { shooter2Motor.setControl(new Follower(ShooterConstants.shooter1CANID, null)); shooter3Motor.setControl(new Follower(ShooterConstants.shooter1CANID, null)); - shooter1AngularVelocity = shooter1Motor.getVelocity(); shooter1AppliedVolts = shooter1Motor.getSupplyVoltage(); shooter1CurrentAmps = shooter1Motor.getSupplyCurrent(); @@ -147,10 +145,12 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.shooter1Current = shooter1CurrentAmps.getValue(); inputs.shooter2Current = shooter2CurrentAmps.getValue(); inputs.shooter3Current = shooter3CurrentAmps.getValue(); + + inputs.wheelVelocity = inputs.shooter1Velocity.times(ShooterConstants.gearRatio); } public void setShooterVelocity(AngularVelocity vel) { - AngularVelocity velocity = vel.times(ShooterConstants.gearRatio); + AngularVelocity velocity = vel.div(ShooterConstants.gearRatio); shooter1Motor.setControl(new MotionMagicVelocityVoltage(velocity)); } @@ -159,7 +159,7 @@ public void setShooterVoltage(Voltage vol) { } public void stopShooter() { - shooter1Motor.set(0); + shooter1Motor.stopMotor(); } public void setHoodPosition(Angle pos) { diff --git a/src/main/java/frc/robot/util/MapleSimUtil.java b/src/main/java/frc/robot/util/MapleSimUtil.java index 546c3be..9b48cf5 100644 --- a/src/main/java/frc/robot/util/MapleSimUtil.java +++ b/src/main/java/frc/robot/util/MapleSimUtil.java @@ -1,11 +1,12 @@ package frc.robot.util; +import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.RadiansPerSecond; import org.ironmaple.simulation.IntakeSimulation; -import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.IntakeSimulation.IntakeSide; +import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.GyroSimulation; import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; @@ -15,18 +16,24 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; -import frc.robot.Constants; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants; public class MapleSimUtil { private static SwerveDriveSimulation drivebaseSimulation; private static IntakeSimulation intakeSimulation; + private static Angle hoodAngle = Degrees.zero(); + private static boolean shooterRunning = false; + private static AngularVelocity shooterVelocity = RadiansPerSecond.zero(); + private static final Command run = Commands.run(MapleSimUtil::periodic) .ignoringDisable(true) @@ -63,14 +70,6 @@ public static SwerveDriveSimulation createSwerve(DriveTrainSimulationConfig conf return drivebaseSimulation; } - public static Pose2d getPosition() { - return drivebaseSimulation.getSimulatedDriveTrainPose(); - } - - public static ChassisSpeeds getFieldChassisSpeeds() { - return drivebaseSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(); - } - public static IntakeSimulation createIntake() { if (drivebaseSimulation == null) throw new IllegalStateException("Intake cannot be created before swerve is!"); @@ -86,6 +85,14 @@ public static IntakeSimulation createIntake() { return intakeSimulation; } + public static Pose2d getPosition() { + return drivebaseSimulation.getSimulatedDriveTrainPose(); + } + + public static ChassisSpeeds getFieldChassisSpeeds() { + return drivebaseSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(); + } + public static int getRemainingGamepieces() { return intakeSimulation.getGamePiecesAmount(); } @@ -102,6 +109,18 @@ public static void loseGamepiece() { intakeSimulation.obtainGamePieceFromIntake(); } + public static void setShooterRunning(boolean running) { + shooterRunning = running; + } + + public static void setShooterVelocity(AngularVelocity velocity) { + shooterVelocity = velocity; + } + + public static void setHoodAngle(Angle angle) { + hoodAngle = angle; + } + public static GyroSimulation getGyroSimulation() { return drivebaseSimulation.getGyroSimulation(); } From 007849b289dc46881326d9a6fba9dd9235969f41 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Thu, 5 Feb 2026 20:56:25 -0800 Subject: [PATCH 07/21] Add Shooter Sim --- .../robot/subsystems/intake/IntakeIOSim.java | 5 ++- .../robot/subsystems/shooter/ShooterIO.java | 5 --- .../subsystems/shooter/ShooterIOSim.java | 45 +++++++++++++++++++ .../subsystems/shooter/ShooterIOTalonFX.java | 6 +-- 4 files changed, 51 insertions(+), 10 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 57745b4..388aaa3 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.intake; import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Robot; import frc.robot.generated.CompTunerConstants; import com.ctre.phoenix6.sim.TalonFXSimState; @@ -40,7 +41,7 @@ public void updateInputs(IntakeIOInputsAutoLogged inputs){ pivotMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); pivotMotorSim.setInputVoltage(pivotMotorSimState.getMotorVoltage()); - pivotMotorSim.update(0.020); //20 millisecond robot sim loop + pivotMotorSim.update(Robot.defaultPeriodSecs); pivotMotorSimState.setRawRotorPosition(pivotMotorSim.getAngularPosition().times(IntakeConstants.motorToPivotGearRatio)); pivotMotorSimState.setRotorVelocity(pivotMotorSim.getAngularVelocity().times(IntakeConstants.motorToPivotGearRatio)); @@ -48,7 +49,7 @@ public void updateInputs(IntakeIOInputsAutoLogged inputs){ rollerMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); rollerMotorSim.setInputVoltage(rollerMotorSimState.getMotorVoltage()); - rollerMotorSim.update(0.020); //20 millisecond robot sim loop + rollerMotorSim.update(Robot.defaultPeriodSecs); //20 millisecond robot sim loop rollerMotorSimState.setRawRotorPosition(rollerMotorSim.getAngularPosition().times(IntakeConstants.rollerGearRatio)); rollerMotorSimState.setRotorVelocity(rollerMotorSim.getAngularVelocity().times(IntakeConstants.rollerGearRatio)); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index c72fca9..64ba2de 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -35,11 +35,6 @@ public static class ShooterIOInputs { public Voltage shooter4Voltage = Volts.zero(); public Current shooter4Current = Amps.zero(); public AngularVelocity shooter4Velocity = RadiansPerSecond.zero(); - - public Angle hoodPosition = Radians.zero(); - public Voltage hoodVoltage = Volts.zero(); - public Current hoodCurrent = Amps.zero(); - public AngularVelocity hoodVelocity = RadiansPerSecond.zero(); public AngularVelocity wheelVelocity = RadiansPerSecond.zero(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java new file mode 100644 index 0000000..c61cb32 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSim.java @@ -0,0 +1,45 @@ +package frc.robot.subsystems.shooter; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.sim.TalonFXSimState; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import frc.robot.Robot; + +public class ShooterIOSim extends ShooterIOTalonFX { + + private final FlywheelSim flywheelSim = new FlywheelSim( + LinearSystemId.createFlywheelSystem( + DCMotor.getKrakenX60Foc(3), 0.01, ShooterConstants.gearRatio + ), + DCMotor.getKrakenX60Foc(3) + ); + + public ShooterIOSim() { + super(new CANBus()); + } + + @Override + public void updateInputs(ShooterIOInputs inputs) { + TalonFXSimState[] motorStates = { + shooter1Motor.getSimState(), + shooter2Motor.getSimState(), + shooter3Motor.getSimState() + }; + + flywheelSim.setInputVoltage(motorStates[0].getMotorVoltage()); + flywheelSim.update(Robot.defaultPeriodSecs); + + for (TalonFXSimState simState : motorStates) { + simState.setSupplyVoltage(RobotController.getBatteryVoltage()); + simState.setRotorVelocity(flywheelSim.getAngularVelocity().times(ShooterConstants.gearRatio)); + simState.setRotorAcceleration(flywheelSim.getAngularAcceleration().times(ShooterConstants.gearRatio)); + } + + super.updateInputs(inputs); + } + +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 1b0b32a..d122421 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -21,9 +21,9 @@ public class ShooterIOTalonFX implements ShooterIO { - private TalonFX shooter1Motor; - private TalonFX shooter2Motor; - private TalonFX shooter3Motor; + protected TalonFX shooter1Motor; + protected TalonFX shooter2Motor; + protected TalonFX shooter3Motor; private Servo hood1; private Servo hood2; From 787da527473863a80a809620526be74ae2e16036 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Thu, 5 Feb 2026 20:57:16 -0800 Subject: [PATCH 08/21] Update --- src/main/java/frc/robot/RobotContainer.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f1d05f8..3c4f71c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -53,6 +53,7 @@ import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.shooter.ShooterIO; +import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.thriftyclimb.ThriftyClimb; import frc.robot.subsystems.thriftyclimb.ThriftyClimbIO; import frc.robot.subsystems.thriftyclimb.ThriftyClimbIOSim; @@ -159,6 +160,8 @@ public RobotContainer() { ); intake_= new IntakeSubsystem(new IntakeIOSim()); + + shooter_ = new Shooter(new ShooterIOSim()); thriftyClimb_ = new ThriftyClimb( new ThriftyClimbIOSim() From 278ea66ba4e5d48b2a1c6410b5588855f75d4ee4 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Thu, 5 Feb 2026 21:00:44 -0800 Subject: [PATCH 09/21] Comment out broken thing --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3c4f71c..b00fbfd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -161,7 +161,7 @@ public RobotContainer() { intake_= new IntakeSubsystem(new IntakeIOSim()); - shooter_ = new Shooter(new ShooterIOSim()); + // shooter_ = new Shooter(new ShooterIOSim()); thriftyClimb_ = new ThriftyClimb( new ThriftyClimbIOSim() From 6f8fb37a1a1fbebbe8ee370c0767594f1d1979aa Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Thu, 5 Feb 2026 22:23:59 -0800 Subject: [PATCH 10/21] Do thing --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 ++ src/main/java/frc/robot/util/MapleSimUtil.java | 1 - 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e141e48..681d558 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -35,7 +35,7 @@ public final class Constants { // falling back to REPLAY. private static final RobotType robotType = RobotType.SIMBOT; - public static final boolean spawnLessFuelInSim = true; + public static final boolean spawnLessFuelInSim = false; public static class DriveConstants { public static final double slowModeJoystickMultiplier = 0.4; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b00fbfd..e2a9a12 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -275,6 +275,8 @@ public RobotContainer() { MapleSimUtil.start(); gamepad_.a().onTrue(Commands.runOnce(() -> { + if (MapleSimUtil.getRemainingGamepieces() == 0) return; + var fuel = new RebuiltFuelOnFly( MapleSimUtil.getPosition().getTranslation(), new Translation2d(), // Initial Robot Position diff --git a/src/main/java/frc/robot/util/MapleSimUtil.java b/src/main/java/frc/robot/util/MapleSimUtil.java index 9b48cf5..999eec2 100644 --- a/src/main/java/frc/robot/util/MapleSimUtil.java +++ b/src/main/java/frc/robot/util/MapleSimUtil.java @@ -2,7 +2,6 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.RadiansPerSecond; import org.ironmaple.simulation.IntakeSimulation; import org.ironmaple.simulation.IntakeSimulation.IntakeSide; From 87b62ab3ee173a560cd5281db5fb0694c644c80d Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Thu, 5 Feb 2026 22:26:09 -0800 Subject: [PATCH 11/21] Fix simulation --- src/main/java/frc/robot/util/MapleSimUtil.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/util/MapleSimUtil.java b/src/main/java/frc/robot/util/MapleSimUtil.java index 999eec2..9b48cf5 100644 --- a/src/main/java/frc/robot/util/MapleSimUtil.java +++ b/src/main/java/frc/robot/util/MapleSimUtil.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.RadiansPerSecond; import org.ironmaple.simulation.IntakeSimulation; import org.ironmaple.simulation.IntakeSimulation.IntakeSide; From cadc45593c8ced55164753df55a92a8b1c8470f7 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Thu, 5 Feb 2026 22:29:52 -0800 Subject: [PATCH 12/21] Fix Mock Shooter Sim --- src/main/java/frc/robot/RobotContainer.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e2a9a12..0890d9b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; import java.util.Arrays; @@ -53,7 +54,6 @@ import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.shooter.ShooterIO; -import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.thriftyclimb.ThriftyClimb; import frc.robot.subsystems.thriftyclimb.ThriftyClimbIO; import frc.robot.subsystems.thriftyclimb.ThriftyClimbIOSim; @@ -274,7 +274,7 @@ public RobotContainer() { if (Constants.getRobot() == RobotType.SIMBOT) { MapleSimUtil.start(); - gamepad_.a().onTrue(Commands.runOnce(() -> { + gamepad_.rightTrigger().whileTrue(Commands.runOnce(() -> { if (MapleSimUtil.getRemainingGamepieces() == 0) return; var fuel = new RebuiltFuelOnFly( @@ -294,7 +294,9 @@ public RobotContainer() { MapleSimUtil.loseGamepiece(); SimulatedArena.getInstance().addGamePieceProjectile(fuel); - })); + }) + .andThen(Commands.waitTime(Seconds.of(0.5))) + .repeatedly()); } // Choosers From 8a141a812ea11592b422c6e22d03412aeae8d53a Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Fri, 6 Feb 2026 00:11:08 -0800 Subject: [PATCH 13/21] Fix simulationgit status! --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../robot/subsystems/intake/IntakeConstants.java | 6 +++--- .../frc/robot/subsystems/intake/IntakeIOSim.java | 15 ++++++++------- .../robot/subsystems/intake/IntakeIOTalonFX.java | 4 ++-- .../frc/robot/subsystems/shooter/ShooterIO.java | 1 - 6 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 681d558..e141e48 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -35,7 +35,7 @@ public final class Constants { // falling back to REPLAY. private static final RobotType robotType = RobotType.SIMBOT; - public static final boolean spawnLessFuelInSim = false; + public static final boolean spawnLessFuelInSim = true; public static class DriveConstants { public static final double slowModeJoystickMultiplier = 0.4; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0890d9b..f7c4cf2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,9 +9,9 @@ import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Milliseconds; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Seconds; import java.util.Arrays; @@ -295,7 +295,7 @@ public RobotContainer() { SimulatedArena.getInstance().addGamePieceProjectile(fuel); }) - .andThen(Commands.waitTime(Seconds.of(0.5))) + .andThen(Commands.waitTime(Milliseconds.of(1000 / 15))) .repeatedly()); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 175ab91..3e39276 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -16,8 +16,8 @@ public final class IntakeConstants { - public static final int rollerMotorCANID= 1;//Temporary - public static final int pivotMotorCANID= 2; //Temporary + public static final int rollerMotorCANID= 8;//Temporary + public static final int pivotMotorCANID= 9; //Temporary public static final int currentLimit= 40; public static final Time currentLimitTime= Seconds.of(1); //Temporary @@ -57,5 +57,5 @@ public final class IntakeConstants { public static final double motorToPivotGearRatio = 2.0; //Unsure yet, will need it for the simulator public static final double rollerGearRatio = 1.0; //Unsure yet, will need it for the simulator public static final MomentOfInertia PIVOT_MOMENTOFINERTIA = KilogramSquareMeters.of(0.01); - public static final MomentOfInertia ROLLER_MOMENTOFINERTIA = KilogramSquareMeters.of(0.01); + public static final MomentOfInertia ROLLER_MOMENTOFINERTIA = KilogramSquareMeters.of(0.001); } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 388aaa3..288eddb 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -1,14 +1,15 @@ package frc.robot.subsystems.intake; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import frc.robot.Robot; -import frc.robot.generated.CompTunerConstants; +import static edu.wpi.first.units.Units.KilogramSquareMeters; import com.ctre.phoenix6.sim.TalonFXSimState; -import edu.wpi.first.math.system.plant.LinearSystemId; + import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.RobotController; -import static edu.wpi.first.units.Units.KilogramSquareMeters; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Robot; +import frc.robot.generated.CompTunerConstants; public class IntakeIOSim extends IntakeIOTalonFX { public final DCMotorSim pivotMotorSim; @@ -21,14 +22,14 @@ public IntakeIOSim() { LinearSystemId.createDCMotorSystem( DCMotor.getKrakenX60Foc(1), IntakeConstants.PIVOT_MOMENTOFINERTIA.in(KilogramSquareMeters), IntakeConstants.motorToPivotGearRatio ), - DCMotor.getKrakenX60Foc(1) //Not sure how many motors + DCMotor.getKrakenX60Foc(1) //Not sure how many motors ); rollerMotorSim= new DCMotorSim( LinearSystemId.createDCMotorSystem( DCMotor.getKrakenX60Foc(1), IntakeConstants.ROLLER_MOMENTOFINERTIA.in(KilogramSquareMeters), IntakeConstants.rollerGearRatio ), - DCMotor.getKrakenX60Foc(1) //Not sure how many motors + DCMotor.getKrakenX60Foc(1) //Not sure how many motors ); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java index 7c31cf4..41991cc 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java @@ -115,8 +115,8 @@ public IntakeIOTalonFX(CANBus pivotBus, CANBus rollerBus) { pivotAngleSignal = pivotMotor.getPosition(); pivotAngularVelocitySignal = pivotMotor.getVelocity(); rollerAngularVelocitySignal = rollerMotor.getVelocity(); - rollerAppliedVoltsSignal = rollerMotor.getSupplyVoltage(); - pivotAppliedVoltsSignal = pivotMotor.getSupplyVoltage(); + rollerAppliedVoltsSignal = rollerMotor.getMotorVoltage(); + pivotAppliedVoltsSignal = pivotMotor.getMotorVoltage(); rollerCurrentAmpsSignal = rollerMotor.getStatorCurrent(); pivotCurrentAmpsSignal = pivotMotor.getStatorCurrent(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 64ba2de..8a6615a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.shooter; import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.Volts; From 901caac10d5be0738a6cf24fcc628fec4ebfb9c2 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Fri, 6 Feb 2026 08:37:17 -0800 Subject: [PATCH 14/21] Separate HoodIO and ShooterIO, (can discuss this later), and fix bugs in the Shooter subsystem. --- src/main/java/frc/robot/RobotContainer.java | 7 ++- .../frc/robot/subsystems/shooter/HoodIO.java | 16 ++++++ .../robot/subsystems/shooter/HoodIOServo.java | 24 ++++++++ .../robot/subsystems/shooter/HoodIOSim.java | 19 +++++++ .../frc/robot/subsystems/shooter/Shooter.java | 56 ++++++++++--------- .../robot/subsystems/shooter/ShooterIO.java | 9 +-- .../subsystems/shooter/ShooterIOTalonFX.java | 53 +++--------------- .../subsystems/thriftyclimb/ThriftyClimb.java | 1 - .../thriftyclimb/ThriftyClimbConstants.java | 2 +- 9 files changed, 108 insertions(+), 79 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/shooter/HoodIO.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/HoodIOServo.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f7c4cf2..4a844cb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -51,9 +51,12 @@ import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.IntakeIOSim; import frc.robot.subsystems.intake.IntakeSubsystem; +import frc.robot.subsystems.shooter.HoodIO; +import frc.robot.subsystems.shooter.HoodIOSim; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.shooter.ShooterIO; +import frc.robot.subsystems.shooter.ShooterIOSim; import frc.robot.subsystems.thriftyclimb.ThriftyClimb; import frc.robot.subsystems.thriftyclimb.ThriftyClimbIO; import frc.robot.subsystems.thriftyclimb.ThriftyClimbIOSim; @@ -161,7 +164,7 @@ public RobotContainer() { intake_= new IntakeSubsystem(new IntakeIOSim()); - // shooter_ = new Shooter(new ShooterIOSim()); + shooter_ = new Shooter(new ShooterIOSim(), new HoodIOSim()); thriftyClimb_ = new ThriftyClimb( new ThriftyClimbIOSim() @@ -256,7 +259,7 @@ public RobotContainer() { } if (shooter_ == null) { - shooter_ = new Shooter(new ShooterIO() {}); + shooter_ = new Shooter(new ShooterIO() {}, new HoodIO() {}); } DriveCommands.configure( diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java new file mode 100644 index 0000000..48dacf9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -0,0 +1,16 @@ +package frc.robot.subsystems.shooter; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.units.measure.Angle; + +public interface HoodIO { + @AutoLog + public static class HoodInputs { + public Angle position; + } + + public default void updateInputs(HoodInputs inputs) {}; + + public default void runPosition(Angle angle) {}; +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIOServo.java b/src/main/java/frc/robot/subsystems/shooter/HoodIOServo.java new file mode 100644 index 0000000..2be5f36 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIOServo.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems.shooter; + +import static edu.wpi.first.units.Units.Degrees; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.Servo; + +public class HoodIOServo implements HoodIO { + + private Servo hoodLeft; + private Servo hoodRight; + + @Override + public void updateInputs(HoodInputs inputs) { + inputs.position = Degrees.of(hoodLeft.getAngle()); + } + + @Override + public void runPosition(Angle angle) { + double degrees = angle.in(Degrees); + hoodLeft.setAngle(degrees); + hoodRight.setAngle(degrees); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java b/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java new file mode 100644 index 0000000..c9cc9a3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.shooter; + +import static edu.wpi.first.units.Units.Degrees; + +import edu.wpi.first.units.measure.Angle; + +public class HoodIOSim implements HoodIO { + private Angle setpointAngle = Degrees.zero(); + + @Override + public void updateInputs(HoodInputs inputs) { + inputs.position = setpointAngle; + } + + @Override + public void runPosition(Angle angle) { + setpointAngle = angle; + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 25c83c3..8c073b1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -28,27 +28,33 @@ public class Shooter extends SubsystemBase { - private final ShooterIO io_; - private final ShooterIOInputsAutoLogged inputs_ = new ShooterIOInputsAutoLogged(); + private final ShooterIO shooterIO; + private final ShooterIOInputsAutoLogged shooterInputs = new ShooterIOInputsAutoLogged(); + + private final HoodIO hoodIO; + private final HoodInputsAutoLogged hoodInputs = new HoodInputsAutoLogged(); private AngularVelocity shooterTarget = RadiansPerSecond.zero(); private Angle hoodTarget = Radians.zero(); - public Shooter(ShooterIO io) { - io_ = io; + public Shooter(ShooterIO ioShooter, HoodIO ioHood) { + this.shooterIO = ioShooter; + this.hoodIO = ioHood; } @Override public void periodic() { - io_.updateInputs(inputs_); - Logger.processInputs("Shooter", inputs_); + shooterIO.updateInputs(shooterInputs); + Logger.processInputs("Shooter", shooterInputs); + hoodIO.updateInputs(hoodInputs); + Logger.processInputs("Shooter/Hood", hoodInputs); - Mechanism3d.measured.setHood(hoodTarget); + Mechanism3d.measured.setHood(hoodInputs.position); Mechanism3d.setpoints.setHood(hoodTarget); if (Constants.getMode() == Mode.SIM) { - MapleSimUtil.setShooterVelocity(inputs_.wheelVelocity); - MapleSimUtil.setHoodAngle(hoodTarget); + MapleSimUtil.setShooterVelocity(shooterInputs.wheelVelocity); + MapleSimUtil.setHoodAngle(hoodInputs.position); } Logger.recordOutput("Shooter/VelocitySetPoint", shooterTarget); @@ -59,36 +65,36 @@ public void periodic() { private void setShooterVelocity(AngularVelocity vel) { shooterTarget = vel; - io_.setShooterVelocity(vel); + shooterIO.setVelocity(vel); } private void setShooterVoltage(Voltage vol) { shooterTarget = RotationsPerSecond.zero(); - io_.setShooterVoltage(vol); + shooterIO.setVoltage(vol); } private void stopShooter() { shooterTarget = RotationsPerSecond.zero(); - io_.stopShooter(); + shooterIO.stop(); } public AngularVelocity getShooterVelocity() { - return inputs_.wheelVelocity; + return shooterInputs.wheelVelocity; } public Voltage getShooterVoltage() { - return inputs_.shooter1Voltage; + return shooterInputs.shooter1Voltage; } public boolean isShooterReady() { - return inputs_.wheelVelocity.isNear(shooterTarget, ShooterConstants.shooterTolerance); + return shooterInputs.wheelVelocity.isNear(shooterTarget, ShooterConstants.shooterTolerance); } public Current getShooterCurrent() { - return (inputs_.shooter1Current) - .plus(inputs_.shooter2Current) - .plus(inputs_.shooter3Current) - .plus(inputs_.shooter4Current); + return (shooterInputs.shooter1Current) + .plus(shooterInputs.shooter2Current) + .plus(shooterInputs.shooter3Current) + .plus(shooterInputs.shooter4Current); } public Command runToVelocityCmd(AngularVelocity vel) { @@ -108,7 +114,7 @@ public Command runVoltageCmd(Voltage vol) { // Hood Methods private void setHoodAngle(Angle pos) { hoodTarget = pos; - io_.setHoodPosition(pos); + hoodIO.runPosition(pos); } public Command hoodToPosCmd(Angle pos) { @@ -129,10 +135,10 @@ private void setSetpoints(AngularVelocity vel, Angle pos) { */ public Command shootCmd() { return startEnd(() -> { - io_.setShooterVelocity(RPM.of(1000)); - io_.setHoodPosition(Degrees.of(45)); + shooterIO.setVelocity(RPM.of(1000)); + hoodIO.runPosition(Degrees.of(45)); }, () -> { - io_.stopShooter(); + shooterIO.stop(); }); } @@ -156,10 +162,10 @@ public Command shooterSysIdDynamic(SysIdRoutine.Direction dir) { private SysIdRoutine shooterIdRoutine() { Voltage step = Volts.of(7); Time to = Seconds.of(10.0); - SysIdRoutine.Config cfg = new SysIdRoutine.Config(null, step, to, (state) -> Logger.recordOutput("HoodSysIdTestState", state.toString())); + SysIdRoutine.Config cfg = new SysIdRoutine.Config(null, step, to, (state) -> Logger.recordOutput("SysIdTestState", state.toString())); SysIdRoutine.Mechanism mfg = new SysIdRoutine.Mechanism( - (volts) -> io_.setShooterVoltage(volts), + (volts) -> shooterIO.setVoltage(volts), null, this); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 8a6615a..2bd953b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -6,7 +6,6 @@ import org.littletonrobotics.junction.AutoLog; -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.Voltage; @@ -40,11 +39,9 @@ public static class ShooterIOInputs { public default void updateInputs(ShooterIOInputs inputs) {} - public default void setShooterVelocity(AngularVelocity vel) {} + public default void setVelocity(AngularVelocity vel) {} - public default void setShooterVoltage(Voltage vol) {} + public default void setVoltage(Voltage vol) {} - public default void stopShooter() {} - - public default void setHoodPosition(Angle pos) {} + public default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index d122421..98439c9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.shooter; +import static frc.robot.util.PhoenixUtil.tryUntilOk; + import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.StatusSignalCollection; @@ -9,15 +11,11 @@ import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.ParentDevice; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.MotorAlignmentValue; -import static edu.wpi.first.units.Units.Degrees; -import static frc.robot.util.PhoenixUtil.tryUntilOk; - -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.Voltage; -import edu.wpi.first.wpilibj.Servo; public class ShooterIOTalonFX implements ShooterIO { @@ -25,9 +23,6 @@ public class ShooterIOTalonFX implements ShooterIO { protected TalonFX shooter2Motor; protected TalonFX shooter3Motor; - private Servo hood1; - private Servo hood2; - private StatusSignalCollection signals; private StatusSignal shooter1AngularVelocity; @@ -48,13 +43,8 @@ public ShooterIOTalonFX(CANBus shooterCANBus) { shooter2Motor = new TalonFX(ShooterConstants.shooter2CANID, shooterCANBus); shooter3Motor = new TalonFX(ShooterConstants.shooter3CANID, shooterCANBus); //shooter4Motor = new TalonFX(ShooterConstants.shooter4CANID, shooterCANBus); - - //hoodMotor = new TalonFX(ShooterConstants.hoodCANID, shooterCANBus); - hood1 = new Servo(0); - hood2 = new Servo(1); final TalonFXConfiguration shooterConfigs = new TalonFXConfiguration(); - // final TalonFXConfiguration hoodConfigs = new TalonFXConfiguration(); // PID shooterConfigs.Slot0.kP = ShooterConstants.PID.shooterkP; @@ -65,22 +55,6 @@ public ShooterIOTalonFX(CANBus shooterCANBus) { shooterConfigs.Slot0.kG = ShooterConstants.PID.shooterkG; shooterConfigs.Slot0.kS = ShooterConstants.PID.shooterkS; - - // hoodConfigs.Slot0.kP = ShooterConstants.PID.hoodkP; - // hoodConfigs.Slot0.kD = ShooterConstants.PID.hoodkD; - // hoodConfigs.Slot0.kI = ShooterConstants.PID.hoodkI; - // hoodConfigs.Slot0.kV = ShooterConstants.PID.hoodkV; - // hoodConfigs.Slot0.kA = ShooterConstants.PID.hoodkA; - // hoodConfigs.Slot0.kG = ShooterConstants.PID.hoodkG; - // hoodConfigs.Slot0.kS = ShooterConstants.PID.hoodkS; - - - // Software Limits - only hood - // hoodConfigs.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - // hoodConfigs.SoftwareLimitSwitch.ForwardSoftLimitThreshold = ShooterConstants.SoftwareLimits.hoodMaxAngle; - // hoodConfigs.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - // hoodConfigs.SoftwareLimitSwitch.ReverseSoftLimitThreshold = ShooterConstants.SoftwareLimits.hoodMinAngle; - // Motion Magic Configurations shooterConfigs.MotionMagic.MotionMagicCruiseVelocity = ShooterConstants.MotionMagic.shooterkMaxVelocity; shooterConfigs.MotionMagic.MotionMagicAcceleration = ShooterConstants.MotionMagic.shooterkMaxAcceleration; @@ -90,17 +64,14 @@ public ShooterIOTalonFX(CANBus shooterCANBus) { shooterConfigs.CurrentLimits.StatorCurrentLimit = ShooterConstants.currentLimit; shooterConfigs.CurrentLimits.StatorCurrentLimitEnable = true; - // hoodConfigs.CurrentLimits.StatorCurrentLimit = ShooterConstants.currentLimit; - // hoodConfigs.CurrentLimits.StatorCurrentLimitEnable = true; - // Similar to our checkError function tryUntilOk(5, () -> shooter1Motor.getConfigurator().apply(shooterConfigs, 0.25)); tryUntilOk(5, () -> shooter2Motor.getConfigurator().apply(shooterConfigs, 0.25)); tryUntilOk(5, () -> shooter3Motor.getConfigurator().apply(shooterConfigs, 0.25)); // Setting other shooter motors as followers - shooter2Motor.setControl(new Follower(ShooterConstants.shooter1CANID, null)); - shooter3Motor.setControl(new Follower(ShooterConstants.shooter1CANID, null)); + shooter2Motor.setControl(new Follower(ShooterConstants.shooter1CANID, MotorAlignmentValue.Aligned)); + shooter3Motor.setControl(new Follower(ShooterConstants.shooter1CANID, MotorAlignmentValue.Aligned)); shooter1AngularVelocity = shooter1Motor.getVelocity(); shooter1AppliedVolts = shooter1Motor.getSupplyVoltage(); @@ -113,7 +84,7 @@ public ShooterIOTalonFX(CANBus shooterCANBus) { shooter3CurrentAmps = shooter3Motor.getSupplyCurrent(); // Status Signal Collection, less repetitive code - StatusSignalCollection signals = new StatusSignalCollection( + signals = new StatusSignalCollection( shooter1AngularVelocity, shooter1AppliedVolts, shooter1CurrentAmps, @@ -149,22 +120,16 @@ public void updateInputs(ShooterIOInputs inputs) { inputs.wheelVelocity = inputs.shooter1Velocity.times(ShooterConstants.gearRatio); } - public void setShooterVelocity(AngularVelocity vel) { + public void setVelocity(AngularVelocity vel) { AngularVelocity velocity = vel.div(ShooterConstants.gearRatio); shooter1Motor.setControl(new MotionMagicVelocityVoltage(velocity)); } - public void setShooterVoltage(Voltage vol) { + public void setVoltage(Voltage vol) { shooter1Motor.setControl(new VoltageOut(vol)); } - public void stopShooter() { + public void stop() { shooter1Motor.stopMotor(); } - - public void setHoodPosition(Angle pos) { - double position = pos.in(Degrees); - hood1.setAngle(position); - hood2.setAngle(position); - } } diff --git a/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimb.java b/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimb.java index 42a9b8c..ffcbc8c 100644 --- a/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimb.java +++ b/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimb.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.thriftyclimb; import static edu.wpi.first.units.Units.Centimeter; -import static edu.wpi.first.units.Units.Meters; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimbConstants.java b/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimbConstants.java index f7619a9..0d5edfc 100644 --- a/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimbConstants.java +++ b/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimbConstants.java @@ -6,7 +6,7 @@ import edu.wpi.first.units.measure.MomentOfInertia; public class ThriftyClimbConstants { - public static final int thriftyClimbId = 1; + public static final int thriftyClimbId = 6; public static final Distance thriftyClimbSpoolDiameter = Inches.of(1.5 * 2); public static final double thriftyGearRatio = 1.0 / 50.0; From e14327362e25750a16077e0e5c340199f91fcaff Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Fri, 6 Feb 2026 09:27:30 -0800 Subject: [PATCH 15/21] Cleanup comments --- .../frc/robot/subsystems/intake/IntakeIOSim.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 288eddb..210bba8 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -20,16 +20,16 @@ public IntakeIOSim() { pivotMotorSim= new DCMotorSim( LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX60Foc(1), IntakeConstants.PIVOT_MOMENTOFINERTIA.in(KilogramSquareMeters), IntakeConstants.motorToPivotGearRatio + DCMotor.getKrakenX60Foc(1), IntakeConstants.PIVOT_MOMENTOFINERTIA.in(KilogramSquareMeters), IntakeConstants.pivotToMotorGearRatio ), - DCMotor.getKrakenX60Foc(1) //Not sure how many motors + DCMotor.getKrakenX60Foc(1) ); rollerMotorSim= new DCMotorSim( LinearSystemId.createDCMotorSystem( DCMotor.getKrakenX60Foc(1), IntakeConstants.ROLLER_MOMENTOFINERTIA.in(KilogramSquareMeters), IntakeConstants.rollerGearRatio ), - DCMotor.getKrakenX60Foc(1) //Not sure how many motors + DCMotor.getKrakenX60Foc(1) ); } @@ -44,13 +44,13 @@ public void updateInputs(IntakeIOInputsAutoLogged inputs){ pivotMotorSim.setInputVoltage(pivotMotorSimState.getMotorVoltage()); pivotMotorSim.update(Robot.defaultPeriodSecs); - pivotMotorSimState.setRawRotorPosition(pivotMotorSim.getAngularPosition().times(IntakeConstants.motorToPivotGearRatio)); - pivotMotorSimState.setRotorVelocity(pivotMotorSim.getAngularVelocity().times(IntakeConstants.motorToPivotGearRatio)); + pivotMotorSimState.setRawRotorPosition(pivotMotorSim.getAngularPosition().times(IntakeConstants.pivotToMotorGearRatio)); + pivotMotorSimState.setRotorVelocity(pivotMotorSim.getAngularVelocity().times(IntakeConstants.pivotToMotorGearRatio)); rollerMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); rollerMotorSim.setInputVoltage(rollerMotorSimState.getMotorVoltage()); - rollerMotorSim.update(Robot.defaultPeriodSecs); //20 millisecond robot sim loop + rollerMotorSim.update(Robot.defaultPeriodSecs); rollerMotorSimState.setRawRotorPosition(rollerMotorSim.getAngularPosition().times(IntakeConstants.rollerGearRatio)); rollerMotorSimState.setRotorVelocity(rollerMotorSim.getAngularVelocity().times(IntakeConstants.rollerGearRatio)); From 4d674523f28c69a090449c5f7661e587797c7bc2 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Fri, 6 Feb 2026 09:45:06 -0800 Subject: [PATCH 16/21] Move shooter sim logic --- src/main/java/frc/robot/RobotContainer.java | 30 --------------- .../robot/subsystems/intake/IntakeIOSim.java | 6 +-- .../java/frc/robot/util/MapleSimUtil.java | 38 +++++++++++++++++++ 3 files changed, 41 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4a844cb..def9651 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,24 +9,18 @@ import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.Milliseconds; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; import java.util.Arrays; -import org.ironmaple.simulation.SimulatedArena; import org.ironmaple.simulation.drivesims.COTS; import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; -import org.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnFly; -import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.net.WebServer; import edu.wpi.first.wpilibj.Filesystem; @@ -276,30 +270,6 @@ public RobotContainer() { // Maple Sim if (Constants.getRobot() == RobotType.SIMBOT) { MapleSimUtil.start(); - - gamepad_.rightTrigger().whileTrue(Commands.runOnce(() -> { - if (MapleSimUtil.getRemainingGamepieces() == 0) return; - - var fuel = new RebuiltFuelOnFly( - MapleSimUtil.getPosition().getTranslation(), - new Translation2d(), // Initial Robot Position - MapleSimUtil.getFieldChassisSpeeds(), - MapleSimUtil.getPosition().getRotation(), - Meters.of(0.5), // Initial Height - MetersPerSecond.of(8), - Degrees.of(50) - ) - .withHitTargetCallBack(() -> System.out.println("Fuel Scored!")) - .withProjectileTrajectoryDisplayCallBack( - poses -> Logger.recordOutput("MapleSim/Trajectory", poses.toArray(Pose3d[]::new)) - ); - - MapleSimUtil.loseGamepiece(); - - SimulatedArena.getInstance().addGamePieceProjectile(fuel); - }) - .andThen(Commands.waitTime(Milliseconds.of(1000 / 15))) - .repeatedly()); } // Choosers diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 210bba8..c2e73cf 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -20,7 +20,7 @@ public IntakeIOSim() { pivotMotorSim= new DCMotorSim( LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX60Foc(1), IntakeConstants.PIVOT_MOMENTOFINERTIA.in(KilogramSquareMeters), IntakeConstants.pivotToMotorGearRatio + DCMotor.getKrakenX60Foc(1), IntakeConstants.PIVOT_MOMENTOFINERTIA.in(KilogramSquareMeters), IntakeConstants.motorToPivotGearRatio ), DCMotor.getKrakenX60Foc(1) ); @@ -44,8 +44,8 @@ public void updateInputs(IntakeIOInputsAutoLogged inputs){ pivotMotorSim.setInputVoltage(pivotMotorSimState.getMotorVoltage()); pivotMotorSim.update(Robot.defaultPeriodSecs); - pivotMotorSimState.setRawRotorPosition(pivotMotorSim.getAngularPosition().times(IntakeConstants.pivotToMotorGearRatio)); - pivotMotorSimState.setRotorVelocity(pivotMotorSim.getAngularVelocity().times(IntakeConstants.pivotToMotorGearRatio)); + pivotMotorSimState.setRawRotorPosition(pivotMotorSim.getAngularPosition().times(IntakeConstants.motorToPivotGearRatio)); + pivotMotorSimState.setRotorVelocity(pivotMotorSim.getAngularVelocity().times(IntakeConstants.motorToPivotGearRatio)); rollerMotorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); diff --git a/src/main/java/frc/robot/util/MapleSimUtil.java b/src/main/java/frc/robot/util/MapleSimUtil.java index 9b48cf5..25984eb 100644 --- a/src/main/java/frc/robot/util/MapleSimUtil.java +++ b/src/main/java/frc/robot/util/MapleSimUtil.java @@ -1,7 +1,11 @@ package frc.robot.util; +import static edu.wpi.first.units.Units.Centimeters; import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Milliseconds; import static edu.wpi.first.units.Units.RadiansPerSecond; import org.ironmaple.simulation.IntakeSimulation; @@ -12,15 +16,20 @@ import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; import org.ironmaple.simulation.seasonspecific.rebuilt2026.Arena2026Rebuilt; +import org.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnFly; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; +import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Constants; @@ -34,17 +43,46 @@ public class MapleSimUtil { private static boolean shooterRunning = false; private static AngularVelocity shooterVelocity = RadiansPerSecond.zero(); + private static final Distance shooterRadius = Centimeters.of(5); + private static final Command run = Commands.run(MapleSimUtil::periodic) .ignoringDisable(true) .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); + private static final Command runShooter = + Commands.runOnce(() -> { + if (MapleSimUtil.getRemainingGamepieces() == 0) return; + + var fuel = new RebuiltFuelOnFly( + getPosition().getTranslation(), + new Translation2d(), // Initial Shooter Position + getFieldChassisSpeeds(), + getPosition().getRotation(), + Meters.of(0.5), // Initial Height + MetersPerSecond.of(shooterVelocity.in(RadiansPerSecond) * shooterRadius.in(Meters)), + hoodAngle + ) + .withHitTargetCallBack(() -> System.out.println("Fuel Scored!")) + .withProjectileTrajectoryDisplayCallBack( + poses -> Logger.recordOutput("MapleSim/Trajectory", poses.toArray(Pose3d[]::new)) + ); + + loseGamepiece(); + + SimulatedArena.getInstance().addGamePieceProjectile(fuel); + }) + .andThen(Commands.waitTime(Milliseconds.of(1000 / 10))) + .repeatedly(); + /** Starts the MapleSim simulation, kicks off a periodic method. */ public static void start() { var rebuilt = (Arena2026Rebuilt) SimulatedArena.getInstance(); rebuilt.setEfficiencyMode(Constants.spawnLessFuelInSim); rebuilt.resetFieldForAuto(); + new Trigger(() -> shooterRunning).whileTrue(runShooter); + CommandScheduler.getInstance().schedule(run); } From d0f562fe1df3fef69b99a64716b2ded38c0b2e48 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Fri, 6 Feb 2026 10:08:38 -0800 Subject: [PATCH 17/21] Shotoer Double Barrel --- src/main/java/frc/robot/RobotContainer.java | 26 +- .../frc/robot/subsystems/hopper/Hopper.java | 269 +++++++++--------- .../frc/robot/subsystems/hopper/HopperIO.java | 10 +- .../java/frc/robot/util/MapleSimUtil.java | 43 +-- 4 files changed, 182 insertions(+), 166 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index def9651..d4ccae8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -42,6 +42,9 @@ import frc.robot.subsystems.drive.ModuleIOMaple; import frc.robot.subsystems.drive.ModuleIOReplay; import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.subsystems.hopper.Hopper; +import frc.robot.subsystems.hopper.HopperIO; +import frc.robot.subsystems.hopper.HopperIOSim; import frc.robot.subsystems.intake.IntakeIO; import frc.robot.subsystems.intake.IntakeIOSim; import frc.robot.subsystems.intake.IntakeSubsystem; @@ -66,9 +69,10 @@ public class RobotContainer { // Subsystems private Drive drivebase_; private AprilTagVision vision_; - private ThriftyClimb thriftyClimb_; private IntakeSubsystem intake_; private Shooter shooter_; + private Hopper hopper_; + private ThriftyClimb climb_; // Choosers private final LoggedDashboardChooser autoChooser_; @@ -159,10 +163,10 @@ public RobotContainer() { intake_= new IntakeSubsystem(new IntakeIOSim()); shooter_ = new Shooter(new ShooterIOSim(), new HoodIOSim()); + + hopper_ = new Hopper(new HopperIOSim()); - thriftyClimb_ = new ThriftyClimb( - new ThriftyClimbIOSim() - ); + climb_ = new ThriftyClimb(new ThriftyClimbIOSim()); break; default: // Comp Bot @@ -243,10 +247,6 @@ public RobotContainer() { cams ); } - - if (thriftyClimb_ == null) { - thriftyClimb_ = new ThriftyClimb(new ThriftyClimbIO() {}); - } if (intake_ == null) { intake_ = new IntakeSubsystem(new IntakeIO() {}); @@ -256,6 +256,14 @@ public RobotContainer() { shooter_ = new Shooter(new ShooterIO() {}, new HoodIO() {}); } + if (hopper_ == null) { + hopper_ = new Hopper(new HopperIO() {}); + } + + if (climb_ == null) { + climb_ = new ThriftyClimb(new ThriftyClimbIO() {}); + } + DriveCommands.configure( drivebase_, () -> -gamepad_.getLeftY(), @@ -289,7 +297,7 @@ public RobotContainer() { // Bind robot actions to commands here. private void configureBindings() { - gamepad_.a().onTrue(thriftyClimb_.toggle()); + gamepad_.a().onTrue(climb_.toggle()); //Testing out each of the commands in the simulator gamepad_.start().onTrue(Commands.either( intake_.deployCmd(), diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index 625047a..ffd6e3f 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -11,148 +11,143 @@ import static edu.wpi.first.units.Units.*; public class Hopper extends SubsystemBase { - - private final HopperIO io; - private final HopperIOInputsAutoLogged inputs = new HopperIOInputsAutoLogged(); - - private AngularVelocity scramblerGoal = RotationsPerSecond.of(0.0); - private AngularVelocity feederGoal = RotationsPerSecond.of(0.0); - - public Hopper(HopperIO io) { - this.io = io; - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Hopper", inputs); - - Logger.recordOutput("Hopper/ScramblerGoalRPS", + + private final HopperIO io; + private final HopperIOInputsAutoLogged inputs = new HopperIOInputsAutoLogged(); + + private AngularVelocity scramblerGoal = RotationsPerSecond.of(0.0); + private AngularVelocity feederGoal = RotationsPerSecond.of(0.0); + + public Hopper(HopperIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Hopper", inputs); + + Logger.recordOutput("Hopper/ScramblerGoalRPS", scramblerGoal.in(RotationsPerSecond)); - Logger.recordOutput("Hopper/FeederGoalRPS", + Logger.recordOutput("Hopper/FeederGoalRPS", feederGoal.in(RotationsPerSecond)); - Logger.recordOutput("Hopper/ScramblerAtGoal", isScramblerAtGoal()); - Logger.recordOutput("Hopper/FeederAtGoal", isFeederAtGoal()); - } - - // Scrambler control - public void setScramblerVelocity(AngularVelocity velocity) { - scramblerGoal = velocity; - io.setScramblerVelocity(velocity); - } - - public void setScramblerVoltage(Voltage voltage) { - scramblerGoal = RotationsPerSecond.of(0.0); - io.setScramblerVoltage(voltage); - } - - public void idleScrambler() { - setScramblerVelocity( - HopperConstants.scramblerMaxVelocity.times(0.15) - ); -} - - public void activeScrambler() { - setScramblerVelocity(HopperConstants.scramblerMaxVelocity); - } - - public void stopScrambler() { - setScramblerVoltage(Volts.of(0.0)); - } - - // Feeder control - public void setFeederVelocity(AngularVelocity velocity) { - feederGoal = velocity; - io.setFeederVelocity(velocity); - } - - public void setFeederVoltage(Voltage voltage) { - feederGoal = RotationsPerSecond.of(0.0); - io.setFeederVoltage(voltage); - } - - public void stopFeeder() { - setFeederVoltage(Volts.of(0.0)); - } - - public void stopAll() { - stopScrambler(); - stopFeeder(); - } - - public void feed() { - setFeederVelocity(HopperConstants.feederMaxVelocity); - } - - public void feedSlow() { - setFeederVelocity( - HopperConstants.feederMaxVelocity.times(0.5) - ); -} - - public void reverseFeed() { - setFeederVelocity( - HopperConstants.feederMaxVelocity.times(-0.5) - ); -} - - public void scrambleAndFeed() { - activeScrambler(); - feed(); - } - - // Readbacks + state checks - public AngularVelocity getScramblerVelocity() { - return inputs.scramblerVelocity; - } - - public AngularVelocity getFeederVelocity() { - return inputs.feederVelocity; - } - - public double getScramblerCurrent() { - return inputs.scramblerCurrent.in(Amps); - } - - public double getFeederCurrent() { - return inputs.feederCurrent.in(Amps); - } - - public boolean isScramblerAtGoal() { - return Math.abs( - inputs.scramblerVelocity.in(RotationsPerSecond) - - scramblerGoal.in(RotationsPerSecond)) < 1.0; - } - - public boolean isFeederAtGoal() { - return Math.abs( - inputs.feederVelocity.in(RotationsPerSecond) - - feederGoal.in(RotationsPerSecond)) < 1.0; - } - - // Command factories - public Command setFeederVoltageCommand(Voltage voltage) { - return Commands.run(() -> setFeederVoltage(voltage), this) + Logger.recordOutput("Hopper/ScramblerAtGoal", isScramblerAtGoal()); + Logger.recordOutput("Hopper/FeederAtGoal", isFeederAtGoal()); + } + + // Scrambler control + public void setScramblerVelocity(AngularVelocity velocity) { + scramblerGoal = velocity; + io.setScramblerVelocity(velocity); + } + + public void setScramblerVoltage(Voltage voltage) { + scramblerGoal = RotationsPerSecond.of(0.0); + io.setScramblerVoltage(voltage); + } + + public void idleScrambler() { + setScramblerVelocity( + HopperConstants.scramblerMaxVelocity.times(0.15) + ); + } + + public void activeScrambler() { + setScramblerVelocity(HopperConstants.scramblerMaxVelocity); + } + + public void stopScrambler() { + setScramblerVoltage(Volts.of(0.0)); + } + + // Feeder control + public void setFeederVelocity(AngularVelocity velocity) { + feederGoal = velocity; + io.setFeederVelocity(velocity); + } + + public void setFeederVoltage(Voltage voltage) { + feederGoal = RotationsPerSecond.of(0.0); + io.setFeederVoltage(voltage); + } + + public void stopFeeder() { + setFeederVoltage(Volts.of(0.0)); + } + + public void stopAll() { + stopScrambler(); + stopFeeder(); + } + + public void feed() { + setFeederVelocity(HopperConstants.feederMaxVelocity); + } + + public void feedSlow() { + setFeederVelocity( + HopperConstants.feederMaxVelocity.times(0.5) + ); + } + + public void reverseFeed() { + setFeederVelocity( + HopperConstants.feederMaxVelocity.times(-0.5) + ); + } + + public void scrambleAndFeed() { + activeScrambler(); + feed(); + } + + // Readbacks + state checks + public AngularVelocity getScramblerVelocity() { + return inputs.scramblerVelocity; + } + + public AngularVelocity getFeederVelocity() { + return inputs.feederVelocity; + } + + public double getScramblerCurrent() { + return inputs.scramblerCurrent.in(Amps); + } + + public double getFeederCurrent() { + return inputs.feederCurrent.in(Amps); + } + + public boolean isScramblerAtGoal() { + return inputs.scramblerVelocity.isNear(scramblerGoal, RotationsPerSecond.one()); + } + + public boolean isFeederAtGoal() { + return inputs.feederVelocity.isNear(feederGoal, RotationsPerSecond.one()); + } + + public Command setFeederVoltageCommand(Voltage voltage) { + return Commands.run(() -> setFeederVoltage(voltage), this) .withName("Hopper.SetFeederVoltage"); - } - - public Command setScramblerVoltageCommand(Voltage voltage) { - return Commands.run(() -> setScramblerVoltage(voltage), this) + } + + public Command setScramblerVoltageCommand(Voltage voltage) { + return Commands.run(() -> setScramblerVoltage(voltage), this) .withName("Hopper.SetScramblerVoltage"); - } - - public Command stopFeederCommand() { - return Commands.run(this::stopFeeder, this) + } + + public Command stopFeederCommand() { + return Commands.run(this::stopFeeder, this) .withName("Hopper.StopFeeder"); - } - - public Command stopScramblerCommand() { - return Commands.run(this::stopScrambler, this) + } + + public Command stopScramblerCommand() { + return Commands.run(this::stopScrambler, this) .withName("Hopper.StopScrambler"); - } - - public Command stopAllCommand() { - return Commands.run(this::stopAll, this) + } + + public Command stopAllCommand() { + return Commands.run(this::stopAll, this) .withName("Hopper.StopAll"); - } + } } diff --git a/src/main/java/frc/robot/subsystems/hopper/HopperIO.java b/src/main/java/frc/robot/subsystems/hopper/HopperIO.java index 431d6fe..5bc8820 100644 --- a/src/main/java/frc/robot/subsystems/hopper/HopperIO.java +++ b/src/main/java/frc/robot/subsystems/hopper/HopperIO.java @@ -1,10 +1,14 @@ package frc.robot.subsystems.hopper; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.DegreesPerSecond; +import static edu.wpi.first.units.Units.Volts; -import edu.wpi.first.units.*; -import edu.wpi.first.units.measure.*; import org.littletonrobotics.junction.AutoLog; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; + public interface HopperIO { @AutoLog public static class HopperIOInputs { diff --git a/src/main/java/frc/robot/util/MapleSimUtil.java b/src/main/java/frc/robot/util/MapleSimUtil.java index 25984eb..d02054a 100644 --- a/src/main/java/frc/robot/util/MapleSimUtil.java +++ b/src/main/java/frc/robot/util/MapleSimUtil.java @@ -15,6 +15,7 @@ import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.simulation.gamepieces.GamePieceProjectile; import org.ironmaple.simulation.seasonspecific.rebuilt2026.Arena2026Rebuilt; import org.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnFly; import org.littletonrobotics.junction.Logger; @@ -29,9 +30,9 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; -import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; public class MapleSimUtil { @@ -52,25 +53,17 @@ public class MapleSimUtil { private static final Command runShooter = Commands.runOnce(() -> { - if (MapleSimUtil.getRemainingGamepieces() == 0) return; - - var fuel = new RebuiltFuelOnFly( - getPosition().getTranslation(), - new Translation2d(), // Initial Shooter Position - getFieldChassisSpeeds(), - getPosition().getRotation(), - Meters.of(0.5), // Initial Height - MetersPerSecond.of(shooterVelocity.in(RadiansPerSecond) * shooterRadius.in(Meters)), - hoodAngle - ) - .withHitTargetCallBack(() -> System.out.println("Fuel Scored!")) - .withProjectileTrajectoryDisplayCallBack( - poses -> Logger.recordOutput("MapleSim/Trajectory", poses.toArray(Pose3d[]::new)) - ); + if (getRemainingGamepieces() == 0) return; + var fuelLeft = createProjectile(Inches.of(3.7)); loseGamepiece(); + SimulatedArena.getInstance().addGamePieceProjectile(fuelLeft); - SimulatedArena.getInstance().addGamePieceProjectile(fuel); + if (getRemainingGamepieces() == 0) return; + + var fuelRight = createProjectile(Inches.of(-3.7)); + loseGamepiece(); + SimulatedArena.getInstance().addGamePieceProjectile(fuelRight); }) .andThen(Commands.waitTime(Milliseconds.of(1000 / 10))) .repeatedly(); @@ -123,6 +116,22 @@ public static IntakeSimulation createIntake() { return intakeSimulation; } + public static GamePieceProjectile createProjectile(Distance yOffset) { + return new RebuiltFuelOnFly( + getPosition().getTranslation(), + new Translation2d(Inches.of(-8), Inches.of(3.7)), // Initial Shooter Position + getFieldChassisSpeeds(), + getPosition().getRotation(), + Inches.of(18.5), // Initial Height + MetersPerSecond.of(shooterVelocity.in(RadiansPerSecond) * shooterRadius.in(Meters)), + hoodAngle + ) + .withHitTargetCallBack(() -> System.out.println("Fuel Scored!")) + .withProjectileTrajectoryDisplayCallBack( + poses -> Logger.recordOutput("MapleSim/Trajectory", poses.toArray(Pose3d[]::new)) + ); + } + public static Pose2d getPosition() { return drivebaseSimulation.getSimulatedDriveTrainPose(); } From b60a7b02ec27c87bd1fd07df4e46580726118576 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Fri, 6 Feb 2026 10:15:26 -0800 Subject: [PATCH 18/21] Integrate Hopper into Simulation --- .../frc/robot/subsystems/hopper/Hopper.java | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index ffd6e3f..8ca02af 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -5,6 +5,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.Mode; +import frc.robot.util.MapleSimUtil; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Voltage; @@ -26,11 +29,17 @@ public Hopper(HopperIO io) { public void periodic() { io.updateInputs(inputs); Logger.processInputs("Hopper", inputs); + + if (Constants.getMode() == Mode.SIM) { + // Set shooter running if feeder and scrambler are both rolling. + MapleSimUtil.setShooterRunning( + inputs.feederVelocity.gt(RadiansPerSecond.zero()) && + inputs.scramblerVelocity.gt(RadiansPerSecond.zero()) + ); + } - Logger.recordOutput("Hopper/ScramblerGoalRPS", - scramblerGoal.in(RotationsPerSecond)); - Logger.recordOutput("Hopper/FeederGoalRPS", - feederGoal.in(RotationsPerSecond)); + Logger.recordOutput("Hopper/ScramblerGoal", scramblerGoal); + Logger.recordOutput("Hopper/FeederGoal", feederGoal); Logger.recordOutput("Hopper/ScramblerAtGoal", isScramblerAtGoal()); Logger.recordOutput("Hopper/FeederAtGoal", isFeederAtGoal()); } From 36c04a5bf1412025caadd76652668754ee2cfe5b Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Fri, 6 Feb 2026 10:57:20 -0800 Subject: [PATCH 19/21] Fix simulation and sequences --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- .../frc/robot/subsystems/hopper/Hopper.java | 2 +- .../subsystems/hopper/HopperConstants.java | 8 ++++---- .../subsystems/intake/IntakeSubsystem.java | 18 ++++++++++++++++-- .../thriftyclimb/ThriftyClimbConstants.java | 2 +- 5 files changed, 25 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d4ccae8..bff186a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -133,8 +133,8 @@ public RobotContainer() { CompTunerConstants.BackLeft.LocationX )), Meters.of(Math.abs( - CompTunerConstants.FrontLeft.LocationX - - CompTunerConstants.FrontRight.LocationX + CompTunerConstants.FrontLeft.LocationY - + CompTunerConstants.FrontRight.LocationY )) ) .withBumperSize(Inches.of(30.75), Inches.of(37.25)); @@ -308,7 +308,7 @@ private void configureBindings() { // While the left trigger is held, we will run the intake. If the intake is stowed, it will also deploy it. gamepad_.leftTrigger().whileTrue( intake_.runIntakeCmd().beforeStarting( - intake_.deployCmd().onlyIf(intake_::isIntakeStowed) + intake_.startDeployCmd().onlyIf(intake_::isIntakeStowed) ) ); diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index 8ca02af..3da47f3 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -37,7 +37,7 @@ public void periodic() { inputs.scramblerVelocity.gt(RadiansPerSecond.zero()) ); } - + Logger.recordOutput("Hopper/ScramblerGoal", scramblerGoal); Logger.recordOutput("Hopper/FeederGoal", feederGoal); Logger.recordOutput("Hopper/ScramblerAtGoal", isScramblerAtGoal()); diff --git a/src/main/java/frc/robot/subsystems/hopper/HopperConstants.java b/src/main/java/frc/robot/subsystems/hopper/HopperConstants.java index fe9f2fd..4475e20 100644 --- a/src/main/java/frc/robot/subsystems/hopper/HopperConstants.java +++ b/src/main/java/frc/robot/subsystems/hopper/HopperConstants.java @@ -8,8 +8,8 @@ public class HopperConstants { //Feeder Constants - public static final int feederMotorCANID = 0; //To be updated - public static final double feederGearRatio = 0.0; //To be updated + public static final int feederMotorCANID = 5; //To be updated + public static final double feederGearRatio = 1.0; //To be updated public static final double feederKP = 0.0; //To be updated public static final double feederKI = 0.0; //To be updated @@ -23,8 +23,8 @@ public class HopperConstants { public static final Current feederCurrentLimit = Amps.of(0.0); //To be updated //Scrambler Constants - public static final int scramblerMotorCANID = 0; //To be updated - public static final double scramblerGearRatio = 0.0; //To be updated + public static final int scramblerMotorCANID = 6; //To be updated + public static final double scramblerGearRatio = 1.0; //To be updated public static final double scramblerKP = 0.0; //To be updated public static final double scramblerKI = 0.0; //To be updated diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 52bde3e..1d8a8c9 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -118,11 +118,19 @@ public Command stowCmd() { * @return */ public Command deployCmd() { - return runOnce(() -> deploy()) - .andThen(Commands.waitUntil(() -> isIntakeDeployed()) + return startDeployCmd() + .andThen(Commands.waitUntil(this::isIntakeDeployed) .withTimeout(2)).withName("Deploy Intake"); } + /** + * Command that starts the deployment of the intake, but doesnt wait until its done. + * @return + */ + public Command startDeployCmd() { + return runOnce(this::deploy); + } + /** * Command that runs the intake until the command ends. * @return The command @@ -131,6 +139,12 @@ public Command runIntakeCmd() { return startEnd(this::startIntaking, this::stopIntaking); } + public Command intakeSequence() { + return runIntakeCmd().beforeStarting( + deployCmd().onlyIf(this::isIntakeStowed) + ); + } + //////////////////////////// /// Sys ID Routine creation/ /// //////////////////////// diff --git a/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimbConstants.java b/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimbConstants.java index 0d5edfc..428ced0 100644 --- a/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimbConstants.java +++ b/src/main/java/frc/robot/subsystems/thriftyclimb/ThriftyClimbConstants.java @@ -6,7 +6,7 @@ import edu.wpi.first.units.measure.MomentOfInertia; public class ThriftyClimbConstants { - public static final int thriftyClimbId = 6; + public static final int thriftyClimbId = 7; public static final Distance thriftyClimbSpoolDiameter = Inches.of(1.5 * 2); public static final double thriftyGearRatio = 1.0 / 50.0; From 7ed3226557f5664659bd1d8ad966705856e8540a Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Fri, 6 Feb 2026 12:04:46 -0800 Subject: [PATCH 20/21] Add Hopper constants and bindings for the hopper --- src/main/java/frc/robot/RobotContainer.java | 8 +- .../frc/robot/subsystems/hopper/Hopper.java | 95 +++++++++++-------- .../subsystems/hopper/HopperCommands.java | 34 ------- .../subsystems/hopper/HopperConstants.java | 6 +- .../frc/robot/subsystems/shooter/Shooter.java | 23 +++-- 5 files changed, 77 insertions(+), 89 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/hopper/HopperCommands.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bff186a..5330559 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -297,8 +297,7 @@ public RobotContainer() { // Bind robot actions to commands here. private void configureBindings() { - gamepad_.a().onTrue(climb_.toggle()); - //Testing out each of the commands in the simulator + // Manually deploying and undeploying the intake. gamepad_.start().onTrue(Commands.either( intake_.deployCmd(), intake_.stowCmd(), @@ -313,7 +312,10 @@ private void configureBindings() { ); // While the right trigger is held, we will shoot into the hub. - gamepad_.rightTrigger().whileTrue(shooter_.shootCmd()); + gamepad_.rightTrigger().whileTrue(shooter_.shootCmd(hopper_)); + + // When the hopper isnt shooting, set it to run its idle velocity. + hopper_.setDefaultCommand(hopper_.idleScrambler()); } private void configureDriveBindings() { diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index 3da47f3..691f2d7 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -45,69 +45,80 @@ public void periodic() { } // Scrambler control - public void setScramblerVelocity(AngularVelocity velocity) { + private void setScramblerVelocity(AngularVelocity velocity) { scramblerGoal = velocity; io.setScramblerVelocity(velocity); } - public void setScramblerVoltage(Voltage voltage) { + private void setScramblerVoltage(Voltage voltage) { scramblerGoal = RotationsPerSecond.of(0.0); io.setScramblerVoltage(voltage); } - public void idleScrambler() { - setScramblerVelocity( - HopperConstants.scramblerMaxVelocity.times(0.15) - ); - } - - public void activeScrambler() { - setScramblerVelocity(HopperConstants.scramblerMaxVelocity); - } - - public void stopScrambler() { + private void stopScrambler() { setScramblerVoltage(Volts.of(0.0)); } // Feeder control - public void setFeederVelocity(AngularVelocity velocity) { + private void setFeederVelocity(AngularVelocity velocity) { feederGoal = velocity; io.setFeederVelocity(velocity); } - public void setFeederVoltage(Voltage voltage) { + private void setFeederVoltage(Voltage voltage) { feederGoal = RotationsPerSecond.of(0.0); io.setFeederVoltage(voltage); } - public void stopFeeder() { + private void stopFeeder() { setFeederVoltage(Volts.of(0.0)); } - public void stopAll() { + private void stopAll() { stopScrambler(); stopFeeder(); } - public void feed() { - setFeederVelocity(HopperConstants.feederMaxVelocity); - } - - public void feedSlow() { - setFeederVelocity( - HopperConstants.feederMaxVelocity.times(0.5) + // Commands + + /** + * Runs the scrambler at its idling speed until the command ends. + * @return + */ + public Command idleScrambler() { + return startEnd( + () -> setFeederVelocity(HopperConstants.feederMaxVelocity.times(0.15)), + this::stopFeeder ); } - - public void reverseFeed() { - setFeederVelocity( - HopperConstants.feederMaxVelocity.times(-0.5) - ); + + /** + * Runs the feeder and scrambler at specified speeds. + * @param feeder + * @param scrambler + * @return + */ + public Command feed(AngularVelocity feeder, AngularVelocity scrambler) { + return startEnd(() -> { + setFeederVelocity(feeder); + setScramblerVelocity(scrambler); + }, this::stopAll); + } + + /** + * Runs the scrambler at its active speed, and the feeder. + * @return + */ + public Command forwardFeed() { + return feed(HopperConstants.scramblerActiveVelocity, HopperConstants.feedingVelocity); } - public void scrambleAndFeed() { - activeScrambler(); - feed(); + /** + * Runs the scramble and feeder in reverse. + * @return + */ + public Command reverseFeed() { + return feed(HopperConstants.scramblerActiveVelocity.times(-1), HopperConstants.feedingVelocity.times(-1)); } // Readbacks + state checks @@ -136,27 +147,27 @@ public boolean isFeederAtGoal() { } public Command setFeederVoltageCommand(Voltage voltage) { - return Commands.run(() -> setFeederVoltage(voltage), this) - .withName("Hopper.SetFeederVoltage"); + return runOnce(() -> setFeederVoltage(voltage)) + .withName("Hopper.SetFeederVoltage"); } public Command setScramblerVoltageCommand(Voltage voltage) { - return Commands.run(() -> setScramblerVoltage(voltage), this) - .withName("Hopper.SetScramblerVoltage"); + return runOnce(() -> setScramblerVoltage(voltage)) + .withName("Hopper.SetScramblerVoltage"); } public Command stopFeederCommand() { - return Commands.run(this::stopFeeder, this) - .withName("Hopper.StopFeeder"); + return runOnce(this::stopFeeder) + .withName("Hopper.StopFeeder"); } public Command stopScramblerCommand() { - return Commands.run(this::stopScrambler, this) - .withName("Hopper.StopScrambler"); + return runOnce(this::stopScrambler) + .withName("Hopper.StopScrambler"); } public Command stopAllCommand() { - return Commands.run(this::stopAll, this) - .withName("Hopper.StopAll"); + return runOnce(this::stopAll) + .withName("Hopper.StopAll"); } } diff --git a/src/main/java/frc/robot/subsystems/hopper/HopperCommands.java b/src/main/java/frc/robot/subsystems/hopper/HopperCommands.java deleted file mode 100644 index fb46ba2..0000000 --- a/src/main/java/frc/robot/subsystems/hopper/HopperCommands.java +++ /dev/null @@ -1,34 +0,0 @@ -package frc.robot.subsystems.hopper; - -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; - -public class HopperCommands { - //Start Motor Commands - public static Command setFeederVoltage(Hopper hopper, Voltage voltage) { - return Commands.run(() -> hopper.setFeederVoltage(voltage), hopper) - .withName("setFeederVoltage"); - } - - public static Command setScramblerVoltage(Hopper hopper, Voltage voltage) { - return Commands.run(() -> hopper.setScramblerVoltage(voltage), hopper) - .withName("setScramblerVoltage"); - } - - //Stop Motor Commands - public static Command stopFeeder(Hopper hopper) { - return Commands.run(hopper::stopFeeder, hopper) - .withName("stopFeeder"); - } - - public static Command stopScrambler(Hopper hopper) { - return Commands.run(hopper::stopScrambler, hopper) - .withName("stopScrambler"); - } - - public static Command stopAll(Hopper hopper) { - return Commands.run(hopper::stopAll, hopper) - .withName("stopAll"); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/hopper/HopperConstants.java b/src/main/java/frc/robot/subsystems/hopper/HopperConstants.java index 4475e20..1bb0538 100644 --- a/src/main/java/frc/robot/subsystems/hopper/HopperConstants.java +++ b/src/main/java/frc/robot/subsystems/hopper/HopperConstants.java @@ -5,13 +5,17 @@ import static edu.wpi.first.units.Units.*; public class HopperConstants { + + // Command Constants + public static final AngularVelocity feedingVelocity = RotationsPerSecond.of(20); + public static final AngularVelocity scramblerActiveVelocity = RotationsPerSecond.of(10); //Feeder Constants public static final int feederMotorCANID = 5; //To be updated public static final double feederGearRatio = 1.0; //To be updated - public static final double feederKP = 0.0; //To be updated + public static final double feederKP = 1.0; //To be updated public static final double feederKI = 0.0; //To be updated public static final double feederKD = 0.0; //To be updated public static final double feederKS = 0.0; //To be updated diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 8c073b1..8a0007d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.Mode; +import frc.robot.subsystems.hopper.Hopper; import frc.robot.util.MapleSimUtil; import frc.robot.util.Mechanism3d; @@ -130,24 +131,28 @@ private void setSetpoints(AngularVelocity vel, Angle pos) { } /** - * Shoot balls into the hub until the command ends. + * Shoot balls from the shooter until the command ends. * @return */ - public Command shootCmd() { - return startEnd(() -> { - shooterIO.setVelocity(RPM.of(1000)); - hoodIO.runPosition(Degrees.of(45)); - }, () -> { - shooterIO.stop(); - }); + public Command shootCmd(Hopper hopper) { + return Commands.parallel( + runDynamicSetpoints(() -> RPM.of(1000), () -> Degrees.of(45)), + hopper.forwardFeed() + ); } public Command runToSetpointsCmd(AngularVelocity vel, Angle pos) { return runOnce(() -> setSetpoints(vel, pos)).andThen(Commands.waitUntil(this::isShooterReady)); } + /** + * Runs supplied setpoints until the command ends, then stops. + * @param vel + * @param pos + * @return + */ public Command runDynamicSetpoints(Supplier vel, Supplier pos ) { - return run(() -> setSetpoints(vel.get(), pos.get())); + return runEnd(() -> setSetpoints(vel.get(), pos.get()), this::stopShooter); } // Sys ID From 8f9a73eaa0ae482f6c83c642ed83245be4c049b0 Mon Sep 17 00:00:00 2001 From: blaze-developer Date: Fri, 6 Feb 2026 12:05:11 -0800 Subject: [PATCH 21/21] Fix import --- .../java/frc/robot/subsystems/hopper/Hopper.java | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index 691f2d7..cf408b9 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -1,17 +1,19 @@ package frc.robot.subsystems.hopper; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; + import org.littletonrobotics.junction.Logger; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Constants.Mode; import frc.robot.util.MapleSimUtil; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Voltage; - -import static edu.wpi.first.units.Units.*; public class Hopper extends SubsystemBase {