From d11a8ad5f7b3e1bdd80448e4efb38ff9ce682f35 Mon Sep 17 00:00:00 2001 From: cadiewynn Date: Sat, 21 Feb 2026 14:33:11 -0500 Subject: [PATCH 1/6] fixed velocity it was very sus. finished intake methods and created button bindings. --- src/main/java/frc/robot/Constants.java | 3 ++ .../java/frc/robot/subsystems/Intake.java | 39 ++++++++++--------- 2 files changed, 24 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 91e0174..5c2b960 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -437,6 +437,9 @@ public class IntakeConstants { public static final Angle MIN_ANGLE = Rotations.of(0.0); public static final Angle MAX_ANGLE = Rotations.of(1); public static final Angle STARTING_ANGLE = Rotations.of(0.0); + public static final Angle PICKUP_ANGLE = Rotations.of(0.0); + public static final Angle STOW_ANGLE = Rotations.of(0.0); + public static final double PICKUP_SPEED = 0.0; public static final Distance WHEEL_RADIUS = Meters.of(0.05); public static final Translation3d OFFSET = Translation3d.kZero; public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.0028125); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 8b962c7..4de5948 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -5,6 +5,7 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; @@ -16,8 +17,6 @@ public class Intake extends SubsystemBase { private FlywheelMechanism _rollerIO; private RotaryMechanism _pivotIO; - double velocity; - double pivotAngle; public double desiredAngle; public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) { @@ -56,26 +55,30 @@ public void stop() { setVelocity(0); } - // public Command intake() { - // return Commands.sequence( - // Commands.run(() -> setVelocity(velocity)), Commands.run(() -> - // setPivotAngle(pivotAngle))); - // } - public void setAngle(Angle angle) { - _pivotIO.runPosition( - angle, - getVelocity(), - IntakePivotConstants.ACCELERATION, - IntakePivotConstants.JERK, - PIDSlot.SLOT_0); - desiredAngle = angle.magnitude(); - } + public Command intake() { + return Commands.sequence( + Commands.run(() -> setVelocity(Constants.IntakeConstants.PICKUP_SPEED)), setPivotAngle(Constants.IntakeConstants.PICKUP_ANGLE)); + } public boolean isIntendedAngle() { return Math.abs(desiredAngle - _pivotIO.getVelocity().in(RotationsPerSecond)) <= IntakePivotConstants.TOLERANCE.magnitude(); } - - @Override + public Command stowAngle() { + return Commands.sequence( + Commands.run(() -> setVelocity(Constants.IntakeConstants.PICKUP_SPEED)), setStowAngle(Constants.IntakeConstants.STOW_ANGLE)); + } + private Command setStowAngle(Angle stowAngle) { + return this.runOnce( + () -> + _pivotIO.runPosition( + stowAngle, + IntakeConstants.CRUISE_VELOCITY, + IntakeConstants.ACCELERATION, + IntakeConstants.JERK, + PIDSlot.SLOT_0)); + } + + @Override public void periodic() {} } From add38a71a34e2e73885e0c93809bfaf47417fb47 Mon Sep 17 00:00:00 2001 From: cadiewynn Date: Sat, 21 Feb 2026 16:28:07 -0500 Subject: [PATCH 2/6] fixing spotless --- src/main/java/frc/robot/Robot.java | 6 ++--- src/main/java/frc/robot/RobotContainer.java | 11 +++----- .../frc/robot/subsystems/BallCounter.java | 11 ++++---- .../java/frc/robot/subsystems/Intake.java | 26 +++++++++++-------- 4 files changed, 27 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index efea88d..63fa59b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,6 +13,7 @@ package frc.robot; +import au.grapplerobotics.CanBridge; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; @@ -31,11 +32,8 @@ * each mode, as described in the TimedRobot documentation. If you change the name of this class or * the package after creating this project, you must also update the build.gradle file in the * project. + * import au.grapplerobotics.CanBridge; */ - import au.grapplerobotics.CanBridge; - - - public class Robot extends LoggedRobot { private Command autonomousCommand; private RobotContainer robotContainer; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5657871..f9a3a54 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -55,13 +55,10 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; - import java.util.Optional; - import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.photonvision.PhotonPoseEstimator.PoseStrategy; - /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -71,7 +68,7 @@ public class RobotContainer { // Subsystems - private final Drive drive; + private final Drive drive; private final Hopper hopper; private final Shooter shooter; private final Climber climber; @@ -259,7 +256,7 @@ public RobotContainer() { break; default: - //Replayed robot, disable IO implementations + // Replayed robot, disable IO implementations drive = new Drive( new GyroIO() {}, @@ -326,7 +323,7 @@ private void configureButtonBindings() { () -> controller.getLeftX(), () -> -controller.getRightX())); - //Lock to 0° when A button is held + // Lock to 0° when A button is held controller .a() .whileTrue( @@ -336,7 +333,7 @@ private void configureButtonBindings() { () -> -controller.getLeftX(), () -> new Rotation2d())); - //Switch to X pattern when X button is pressed + // Switch to X pattern when X button is pressed controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); // Reset gyro to 0° when B button is pressed diff --git a/src/main/java/frc/robot/subsystems/BallCounter.java b/src/main/java/frc/robot/subsystems/BallCounter.java index 513518b..874c405 100644 --- a/src/main/java/frc/robot/subsystems/BallCounter.java +++ b/src/main/java/frc/robot/subsystems/BallCounter.java @@ -6,7 +6,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.ArrayList; -import java.util.Iterator; import java.util.List; public class BallCounter extends SubsystemBase { @@ -57,7 +56,9 @@ public void shootBall() { public boolean ballShot() { Measurement measurement = _laserCAN.getMeasurement(); - return measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT && measurement.distance_mm <= 30; + return measurement != null + && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT + && measurement.distance_mm <= 30; } public double calculateFireRate() { @@ -87,12 +88,12 @@ public void periodic() { SmartDashboard.putNumber("Balls Fired", getBallsShot()); SmartDashboard.putNumber("Ball Fire Rate (per second)", Math.round(fireRate * 100.0) / 100.0); - SmartDashboard.putNumber("Seconds Since Last Fire", Math.round(secondsSinceLastFired() * 100.0) / 100.0); + SmartDashboard.putNumber( + "Seconds Since Last Fire", Math.round(secondsSinceLastFired() * 100.0) / 100.0); if (!isBlocked && !canFire) { canFire = true; - } - else if (isBlocked && canFire) { + } else if (isBlocked && canFire) { shootBall(); canFire = false; } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 4de5948..3c624f8 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -57,19 +57,23 @@ public void stop() { public Command intake() { return Commands.sequence( - Commands.run(() -> setVelocity(Constants.IntakeConstants.PICKUP_SPEED)), setPivotAngle(Constants.IntakeConstants.PICKUP_ANGLE)); - } + Commands.run(() -> setVelocity(Constants.IntakeConstants.PICKUP_SPEED)), + setPivotAngle(Constants.IntakeConstants.PICKUP_ANGLE)); + } public boolean isIntendedAngle() { return Math.abs(desiredAngle - _pivotIO.getVelocity().in(RotationsPerSecond)) <= IntakePivotConstants.TOLERANCE.magnitude(); } - public Command stowAngle() { - return Commands.sequence( - Commands.run(() -> setVelocity(Constants.IntakeConstants.PICKUP_SPEED)), setStowAngle(Constants.IntakeConstants.STOW_ANGLE)); - } - private Command setStowAngle(Angle stowAngle) { - return this.runOnce( + + public Command stowAngle() { + return Commands.sequence( + Commands.run(() -> setVelocity(Constants.IntakeConstants.PICKUP_SPEED)), + setStowAngle(Constants.IntakeConstants.STOW_ANGLE)); + } + + private Command setStowAngle(Angle stowAngle) { + return this.runOnce( () -> _pivotIO.runPosition( stowAngle, @@ -77,8 +81,8 @@ private Command setStowAngle(Angle stowAngle) { IntakeConstants.ACCELERATION, IntakeConstants.JERK, PIDSlot.SLOT_0)); - } - - @Override + } + + @Override public void periodic() {} } From 176f999f68bfb7efd6fad6be2edb43542d0b1f45 Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Mon, 23 Feb 2026 17:57:22 -0500 Subject: [PATCH 3/6] fix spotless --- src/main/java/frc/robot/Robot.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 63fa59b..55f03ce 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -32,7 +32,6 @@ * each mode, as described in the TimedRobot documentation. If you change the name of this class or * the package after creating this project, you must also update the build.gradle file in the * project. - * import au.grapplerobotics.CanBridge; */ public class Robot extends LoggedRobot { private Command autonomousCommand; From da466c4524fce42cdacaf7479c1dbdaf4678f12a Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Mon, 23 Feb 2026 18:12:39 -0500 Subject: [PATCH 4/6] fix build issue --- src/main/java/frc/robot/Constants.java | 6 ++++-- src/main/java/frc/robot/subsystems/Intake.java | 17 ++++++++--------- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b14dbb9..c5aa856 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -162,6 +162,7 @@ public class HopperConstants { public static final String MOTOR_NAME = "Hopper Roller"; // CHANGE TO PROPER RPMS !!!! + public static final double SLOW_SPEED_RPM = 0.0; public static final double FAST_SPEED_RPM = 0.0; public static final double REVERSE_SPEED_RPM = 0.0; @@ -437,8 +438,6 @@ public class IntakeFlywheelConstants { public static final Angle MIN_ANGLE = Rotations.of(0.0); public static final Angle MAX_ANGLE = Rotations.of(1); public static final Angle STARTING_ANGLE = Rotations.of(0.0); - public static final Angle PICKUP_ANGLE = Rotations.of(0.0); - public static final Angle STOW_ANGLE = Rotations.of(0.0); public static final double PICKUP_SPEED = 0.0; public static final Distance WHEEL_RADIUS = Meters.of(0.05); public static final Translation3d OFFSET = Translation3d.kZero; @@ -571,6 +570,9 @@ public static TalonFXConfiguration getFXConfig() { public class IntakePivotConstants { public static final String NAME = "Intake"; + public static final Angle PICKUP_ANGLE = Rotations.of(0.0); + public static final Angle STOW_ANGLE = Rotations.of(0.0); + public static final Angle TOLERANCE = Degrees.of(1.0); public static final AngularVelocity CRUISE_VELOCITY = RadiansPerSecond.of(10); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 26ccc72..e38b78f 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -14,7 +14,6 @@ import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism; import frc.lib.W8.mechanisms.rotary.RotaryMechanism; -import frc.robot.Constants; import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakePivotConstants; import org.littletonrobotics.junction.Logger; @@ -33,7 +32,7 @@ public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) { public void setVelocity(double velocity) { AngularVelocity angVelo = RotationsPerSecond.of(velocity); - _rollerIO.runVelocity(angVelo, Constants.IntakeFlywheelConstants.ACCELERATION, PIDSlot.SLOT_0); + _rollerIO.runVelocity(angVelo, IntakeFlywheelConstants.ACCELERATION, PIDSlot.SLOT_0); } public Command setPivotAngle(Angle pivotAngle) { @@ -62,8 +61,8 @@ public void stop() { public Command intake() { return Commands.sequence( - Commands.run(() -> setVelocity(Constants.IntakeConstants.PICKUP_SPEED)), - setPivotAngle(Constants.IntakeConstants.PICKUP_ANGLE)); + Commands.run(() -> setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)), + setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)); } public boolean isIntendedAngle() { @@ -73,8 +72,8 @@ public boolean isIntendedAngle() { public Command stowAngle() { return Commands.sequence( - Commands.run(() -> setVelocity(Constants.IntakeConstants.PICKUP_SPEED)), - setStowAngle(Constants.IntakeConstants.STOW_ANGLE)); + Commands.run(() -> setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)), + setStowAngle(IntakePivotConstants.STOW_ANGLE)); } private Command setStowAngle(Angle stowAngle) { @@ -82,9 +81,9 @@ private Command setStowAngle(Angle stowAngle) { () -> _pivotIO.runPosition( stowAngle, - IntakeConstants.CRUISE_VELOCITY, - IntakeConstants.ACCELERATION, - IntakeConstants.JERK, + IntakePivotConstants.CRUISE_VELOCITY, + IntakePivotConstants.ACCELERATION, + IntakePivotConstants.JERK, PIDSlot.SLOT_0)); } From 643e48b24473e573224e0685c7c5b0a608ee0f2d Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Mon, 23 Feb 2026 18:13:43 -0500 Subject: [PATCH 5/6] renamed command --- src/main/java/frc/robot/subsystems/Intake.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index e38b78f..672bf1b 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -70,7 +70,7 @@ public boolean isIntendedAngle() { <= IntakePivotConstants.TOLERANCE.magnitude(); } - public Command stowAngle() { + public Command stowAndStopRollers() { return Commands.sequence( Commands.run(() -> setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)), setStowAngle(IntakePivotConstants.STOW_ANGLE)); From d5cccfffbac58342e28fa1fdacfe6bf1022b9530 Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Mon, 23 Feb 2026 18:14:48 -0500 Subject: [PATCH 6/6] spotless --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c5aa856..dfbdfde 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -162,7 +162,7 @@ public class HopperConstants { public static final String MOTOR_NAME = "Hopper Roller"; // CHANGE TO PROPER RPMS !!!! - + public static final double SLOW_SPEED_RPM = 0.0; public static final double FAST_SPEED_RPM = 0.0; public static final double REVERSE_SPEED_RPM = 0.0;