From 548792cb809601c901bb61d0a992c5d96c07fcd5 Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Wed, 25 Feb 2026 19:27:31 -0500 Subject: [PATCH 1/4] 2 / 25 / 26 can now collect and fire balls in fuel sim --- src/main/java/frc/robot/Constants.java | 5 ++-- src/main/java/frc/robot/Robot.java | 11 ++++++-- src/main/java/frc/robot/RobotContainer.java | 8 +++--- .../java/frc/robot/subsystems/Intake.java | 13 +++++++++- .../java/frc/robot/subsystems/Shooter.java | 25 +++++++++++++++++++ 5 files changed, 53 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7dddebb..e9756a4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -573,7 +573,7 @@ public class IntakePivotConstants { public static final AngularVelocity CRUISE_VELOCITY = RadiansPerSecond.of(10); public static final AngularAcceleration ACCELERATION = RadiansPerSecondPerSecond.of(100); public static final Velocity JERK = - RadiansPerSecondPerSecond.per(Second).of(0); + RadiansPerSecondPerSecond.per(Second).of(0.1); private static final double ROTOR_TO_SENSOR = (50.0 / 1.0); private static final double SENSOR_TO_MECHANISM = 1.0; @@ -592,7 +592,8 @@ public class IntakePivotConstants { // Positional PID public static final Slot0Configs SLOT_0_CONFIG = - new Slot0Configs().withKP(10.0).withKI(2.0).withKD(8).withKS(0.07).withKV(0.1); + new Slot0Configs().withKP(100.0).withKI(0.0).withKD(0).withKS(0.07).withKV(0.1); + // ^^^ CHANGE public static TalonFXConfiguration getFXConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9789541..c841b88 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -18,6 +18,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.lib.Rebuilt2026.FuelSim; @@ -45,8 +46,8 @@ public class Robot extends LoggedRobot { private Command autonomousCommand; - private RobotContainer robotContainer; - public FuelSim fuelSim = new FuelSim(); + public static RobotContainer robotContainer; + public static FuelSim fuelSim = new FuelSim(); public Robot() { CanBridge.runTCP(); @@ -196,6 +197,10 @@ public void simulationInit() { robotContainer.drive ::getChassisSpeeds); // Supplier of field-centric chassis speed + fuelSim.registerIntake(0.4, 0.5, -0.4, 0.4, + () -> robotContainer.intake.canIntake(), + () -> {robotContainer.intake.simBalls++;}); + fuelSim.start(); // enables the simulation to run (updateSim must still be called periodically) fuelSim.enableAirResistance(); // an additional drag force will be applied to fuel in physics @@ -208,5 +213,7 @@ public void simulationPeriodic() { fuelSim.updateSim(); Logger.recordOutput("Zero Pose", new Pose3d()); + + SmartDashboard.putNumber("Sim Balls", (double) robotContainer.intake.simBalls); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e3c65d3..f14ce6a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -74,11 +74,11 @@ public class RobotContainer { // Subsystems - public final Drive drive; + public final Drive drive; private final Hopper hopper; private final Shooter shooter; private final Climber climber; - private final Intake intake; + public final Intake intake; private final BallCounter ballCounter; private final Vision vision; @@ -350,8 +350,8 @@ private void configureButtonBindings() { .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); controller - .y() - .onTrue(Commands.runOnce(() -> intake.setVelocity(1))); + .rightTrigger() + .onTrue(Commands.runOnce(() -> shooter.simShoot())); } /** diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 896afd8..d88c045 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Degree; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Volts; @@ -26,11 +27,14 @@ public class Intake extends SubsystemBase { private RotaryMechanism _pivotIO; double velocity; double pivotAngle; + public int simBalls; public double desiredAngle; public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) { _rollerIO = rollerIO; _pivotIO = pivotIO; + + simBalls = 0; } // Velocity of Rollers @@ -84,11 +88,18 @@ public boolean isIntendedAngle() { <= IntakePivotConstants.TOLERANCE.magnitude(); } + public boolean canIntake() + { + return _pivotIO.getPosition().in(Degree) > (IntakePivotConstants.MAX_ANGLE.in(Degree) - 10) && simBalls < 45 && simBalls >= 0; + } + public void periodic() { + if (_pivotIO.getPosition().in(Degree) < IntakePivotConstants.MAX_ANGLE.in(Degree)) _pivotIO.runVoltage(Volts.of(0.25)); + _pivotIO.periodic(); Logger.recordOutput("3DField/1_Intake", new Pose3d(new Translation3d(0.3085,0.0,0.175), new Rotation3d(0, _pivotIO.getPosition().in(Radians), 0))); Logger.recordOutput("3DField/2_Hopper", new Pose3d(new Translation3d(Math.sin(_pivotIO.getPosition().in(Radians)*0.1055),0, 0), new Rotation3d(0, 0, 0))); - // _pivotIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*0.25)); --- Tests the pivot + //_pivotIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*0.25)); //--- Tests the pivot } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 5f91890..cf74ad2 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -7,8 +7,10 @@ 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.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.Units; @@ -24,6 +26,7 @@ import frc.robot.Constants.FeederConstants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.ShooterConstants; +import frc.robot.Robot; public class Shooter extends SubsystemBase { @@ -128,8 +131,30 @@ public Command shoot(double velocity) { Commands.runOnce(() -> setFlywheelVelocity(0))); } + public void simShoot() + { + if (Robot.robotContainer.intake.simBalls <= 0) return; + + double flywheelSpeed = 6; + Translation2d robotPose2d = Robot.robotContainer.drive.getPose().getTranslation(); + double Yaw = Robot.robotContainer.drive.getPose().getRotation().getRadians(); + Pose3d robotPose3d = new Pose3d(new Translation3d(robotPose2d.getX(),robotPose2d.getY(),0), new Rotation3d(robotPose2d.getAngle())); + Pose3d shooterPose3d = new Pose3d(new Translation3d(-0.0075,0.0,0.523), new Rotation3d(0, _hood.getPosition().in(Radians), 0)); + + double V_xy = Math.sin(Math.PI/2-(_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians)))*flywheelSpeed; + System.out.println(V_xy*Math.cos(Yaw)); + System.out.println(V_xy*Math.sin(Yaw)); + System.out.println(Math.sin(_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))); + + Robot.fuelSim.spawnFuel(robotPose3d.plus(new Transform3d(shooterPose3d.getX(), shooterPose3d.getY(), shooterPose3d.getZ(), new Rotation3d(0, 0, 0))).getTranslation(), new Translation3d(V_xy*Math.cos(Yaw), V_xy*Math.sin(Yaw), Math.sin(Math.PI/2-(_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) *flywheelSpeed)); + Robot.robotContainer.intake.simBalls--; + } + public void periodic() { _hood.periodic(); + // _feeder.periodic(); + // _flywheel.periodic(); + double pitch = Math.toRadians(Math.abs(Math.sin(Timer.getFPGATimestamp())*45)); // Placeholder for position // The pitch of the Rotation3D should be '_hood.getPosition().in(Radians)', change after fixing motor configs. From ac9823e24f83e1c668d6323a2ead6c2303c11650 Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Wed, 25 Feb 2026 19:34:21 -0500 Subject: [PATCH 2/4] fixed small thing --- src/main/java/frc/robot/subsystems/Intake.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 7c4e6e5..1c65dc8 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.Degree; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; From d34bb84350e92957de05ae02483410bc0d06ce27 Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Wed, 25 Feb 2026 20:03:13 -0500 Subject: [PATCH 3/4] Cleaned up a little --- src/main/java/frc/robot/RobotContainer.java | 8 ++++++++ src/main/java/frc/robot/subsystems/Shooter.java | 3 --- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 14e5df3..f6be315 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -346,6 +346,14 @@ private void configureButtonBindings() { controller .rightTrigger() .onTrue(Commands.runOnce(() -> shooter.simShoot())); + + controller + .leftTrigger() + .onTrue(Commands.runOnce(() -> { + Robot.fuelSim.clearFuel(); + Robot.fuelSim.spawnStartingFuel(); + intake.simBalls = 0; + })); } /** diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 700267d..3ad1ebe 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -141,9 +141,6 @@ public void simShoot() Pose3d shooterPose3d = new Pose3d(new Translation3d(-0.0075,0.0,0.523), new Rotation3d(0, _hood.getPosition().in(Radians), 0)); double V_xy = Math.sin(Math.PI/2-(_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians)))*flywheelSpeed; - System.out.println(V_xy*Math.cos(Yaw)); - System.out.println(V_xy*Math.sin(Yaw)); - System.out.println(Math.sin(_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))); Robot.fuelSim.spawnFuel(robotPose3d.plus(new Transform3d(shooterPose3d.getX(), shooterPose3d.getY(), shooterPose3d.getZ(), new Rotation3d(0, 0, 0))).getTranslation(), new Translation3d(V_xy*Math.cos(Yaw), V_xy*Math.sin(Yaw), Math.sin(Math.PI/2-(_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) *flywheelSpeed)); Robot.robotContainer.intake.simBalls--; From f9b88eb9b314be95d8b15a7b3d62e050d71f3366 Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Thu, 26 Feb 2026 18:19:30 -0500 Subject: [PATCH 4/4] spotless --- src/main/java/frc/robot/Constants.java | 3 +- src/main/java/frc/robot/Robot.java | 16 ++++-- src/main/java/frc/robot/RobotContainer.java | 17 +++--- .../java/frc/robot/subsystems/Intake.java | 12 +++-- .../java/frc/robot/subsystems/Shooter.java | 52 ++++++++++++++----- 5 files changed, 66 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 99569da..d82af9b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -599,7 +599,8 @@ public class IntakePivotConstants { // Positional PID public static final Slot0Configs SLOT_0_CONFIG = new Slot0Configs().withKP(100.0).withKI(0.0).withKD(0).withKS(0.07).withKV(0.1); - // ^^^ CHANGE + + // ^^^ CHANGE public static TalonFXConfiguration getFXConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e2e3ff8..112672b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -21,10 +21,10 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.lib.Rebuilt2026.FuelSim; @@ -199,9 +199,15 @@ public void simulationInit() { robotContainer.drive ::getChassisSpeeds); // Supplier of field-centric chassis speed - fuelSim.registerIntake(0.4, 0.5, -0.4, 0.4, - () -> robotContainer.intake.canIntake(), - () -> {robotContainer.intake.simBalls++;}); + fuelSim.registerIntake( + 0.4, + 0.5, + -0.4, + 0.4, + () -> robotContainer.intake.canIntake(), + () -> { + robotContainer.intake.simBalls++; + }); fuelSim.start(); // enables the simulation to run (updateSim must still be called periodically) @@ -215,7 +221,7 @@ public void simulationPeriodic() { fuelSim.updateSim(); Logger.recordOutput("Zero Pose", new Pose3d()); - + SmartDashboard.putNumber("Sim Balls", (double) robotContainer.intake.simBalls); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 34a9873..330cbdd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -40,7 +40,6 @@ import frc.robot.commands.DriveCommands; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.BallCounter; -import frc.robot.subsystems.Climber; import frc.robot.subsystems.Hopper; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; @@ -315,17 +314,17 @@ private void configureButtonBindings() { .x() .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); - controller - .rightTrigger() - .onTrue(Commands.runOnce(() -> shooter.simShoot())); + controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot())); controller .leftTrigger() - .onTrue(Commands.runOnce(() -> { - Robot.fuelSim.clearFuel(); - Robot.fuelSim.spawnStartingFuel(); - intake.simBalls = 0; - })); + .onTrue( + Commands.runOnce( + () -> { + Robot.fuelSim.clearFuel(); + Robot.fuelSim.spawnStartingFuel(); + intake.simBalls = 0; + })); controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(1))); controller.leftBumper().onTrue(intake.intake()); controller.rightBumper().onTrue(intake.stowAndStopRollers()); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 2442832..414cebc 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -77,9 +77,10 @@ public boolean isIntendedAngle() { <= IntakePivotConstants.TOLERANCE.magnitude(); } - public boolean canIntake() - { - return _pivotIO.getPosition().in(Degree) > (IntakePivotConstants.MAX_ANGLE.in(Degree) - 10) && simBalls < 45 && simBalls >= 0; + public boolean canIntake() { + return _pivotIO.getPosition().in(Degree) > (IntakePivotConstants.MAX_ANGLE.in(Degree) - 10) + && simBalls < 45 + && simBalls >= 0; } public Command stowAndStopRollers() { @@ -101,7 +102,8 @@ private Command setStowAngle(Angle stowAngle) { @Override public void periodic() { - if (_pivotIO.getPosition().in(Degree) < IntakePivotConstants.MAX_ANGLE.in(Degree)) _pivotIO.runVoltage(Volts.of(0.25)); + if (_pivotIO.getPosition().in(Degree) < IntakePivotConstants.MAX_ANGLE.in(Degree)) + _pivotIO.runVoltage(Volts.of(0.25)); _pivotIO.periodic(); Logger.recordOutput( @@ -115,6 +117,6 @@ public void periodic() { new Translation3d(Math.sin(_pivotIO.getPosition().in(Radians) * 0.1055), 0, 0), new Rotation3d(0, 0, 0))); - //_pivotIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*0.25)); //--- Tests the pivot + // _pivotIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*0.25)); //--- Tests the pivot } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 3ad1ebe..31ee0ca 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -5,7 +5,6 @@ import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Volts; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -130,19 +129,39 @@ public Command shoot(double velocity) { Commands.runOnce(() -> setFlywheelVelocity(0))); } - public void simShoot() - { + public void simShoot() { if (Robot.robotContainer.intake.simBalls <= 0) return; double flywheelSpeed = 6; Translation2d robotPose2d = Robot.robotContainer.drive.getPose().getTranslation(); double Yaw = Robot.robotContainer.drive.getPose().getRotation().getRadians(); - Pose3d robotPose3d = new Pose3d(new Translation3d(robotPose2d.getX(),robotPose2d.getY(),0), new Rotation3d(robotPose2d.getAngle())); - Pose3d shooterPose3d = new Pose3d(new Translation3d(-0.0075,0.0,0.523), new Rotation3d(0, _hood.getPosition().in(Radians), 0)); - - double V_xy = Math.sin(Math.PI/2-(_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians)))*flywheelSpeed; - - Robot.fuelSim.spawnFuel(robotPose3d.plus(new Transform3d(shooterPose3d.getX(), shooterPose3d.getY(), shooterPose3d.getZ(), new Rotation3d(0, 0, 0))).getTranslation(), new Translation3d(V_xy*Math.cos(Yaw), V_xy*Math.sin(Yaw), Math.sin(Math.PI/2-(_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) *flywheelSpeed)); + Pose3d robotPose3d = + new Pose3d( + new Translation3d(robotPose2d.getX(), robotPose2d.getY(), 0), + new Rotation3d(robotPose2d.getAngle())); + Pose3d shooterPose3d = + new Pose3d( + new Translation3d(-0.0075, 0.0, 0.523), + new Rotation3d(0, _hood.getPosition().in(Radians), 0)); + + double V_xy = + Math.sin(Math.PI / 2 - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) + * flywheelSpeed; + + Robot.fuelSim.spawnFuel( + robotPose3d + .plus( + new Transform3d( + shooterPose3d.getX(), + shooterPose3d.getY(), + shooterPose3d.getZ(), + new Rotation3d(0, 0, 0))) + .getTranslation(), + new Translation3d( + V_xy * Math.cos(Yaw), + V_xy * Math.sin(Yaw), + Math.sin(Math.PI / 2 - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians))) + * flywheelSpeed)); Robot.robotContainer.intake.simBalls--; } @@ -150,11 +169,16 @@ public void periodic() { _hood.periodic(); // _feeder.periodic(); // _flywheel.periodic(); - - double pitch = Math.toRadians(Math.abs(Math.sin(Timer.getFPGATimestamp())*45)); // Placeholder for position - - // The pitch of the Rotation3D should be '_hood.getPosition().in(Radians)', change after fixing motor configs. - Logger.recordOutput("3DField/3_Hood", new Pose3d(new Translation3d(-0.0075,0.0,0.523), new Rotation3d(0, pitch, 0))); + + double pitch = + Math.toRadians( + Math.abs(Math.sin(Timer.getFPGATimestamp()) * 45)); // Placeholder for position + + // The pitch of the Rotation3D should be '_hood.getPosition().in(Radians)', change after fixing + // motor configs. + Logger.recordOutput( + "3DField/3_Hood", + new Pose3d(new Translation3d(-0.0075, 0.0, 0.523), new Rotation3d(0, pitch, 0))); _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); }