From b4a0f573db2ebc849d38b58f5d5485d5f6626516 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Thu, 26 Feb 2026 01:19:08 -0600 Subject: [PATCH 001/102] Updated AdvantageKit and WhatTime. Added distance to hub method to Drive.java. Changed intake subsystem to use kraken built in motor encoder. Worked a lot on the shooter subsystem (it's somewhat functional probably). Let's pretend it isn't 1 in the morning. --- src/main/java/frc/robot/BuildConstants.java | 14 ++-- src/main/java/frc/robot/Robot.java | 1 - .../frc/robot/subsystems/drive/Drive.java | 17 +++++ .../mechanisms/intake/IntakeSubsystem.java | 15 ++-- .../mechanisms/shooter/ShooterDataPoint.java | 17 +++++ .../mechanisms/shooter/ShooterSubsystem.java | 71 ++++++++++++++----- .../mechanisms/shooter/UpperLowerPoint.java | 38 ++++++++++ vendordeps/AdvantageKit.json | 66 ++++++++--------- vendordeps/WhatTime.json | 4 +- 9 files changed, 176 insertions(+), 67 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterDataPoint.java create mode 100644 src/main/java/frc/robot/subsystems/mechanisms/shooter/UpperLowerPoint.java diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 8ae32ba3..eccfb282 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,13 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026 Roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 122; - public static final String GIT_SHA = "1bf40e3766827d608ce15c35ab7e1da9d04e127b"; - public static final String GIT_DATE = "2026-02-25 19:11:15 CST"; - public static final String GIT_BRANCH = "feature/driving-locked-to-hub"; - public static final String BUILD_DATE = "2026-02-25 19:14:38 CST"; - public static final long BUILD_UNIX_TIME = 1772068478558L; - public static final int DIRTY = 0; + public static final int GIT_REVISION = 125; + public static final String GIT_SHA = "2b20aba8b0841b8d59dc828f7f6cfadd181b217d"; + public static final String GIT_DATE = "2026-02-25 19:31:48 CST"; + public static final String GIT_BRANCH = "feature/intake-and-shooter"; + public static final String BUILD_DATE = "2026-02-26 01:17:31 CST"; + public static final long BUILD_UNIX_TIME = 1772090251769L; + public static final int DIRTY = 1; private BuildConstants(){} } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 33f8dfae..a5d665b6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -103,7 +103,6 @@ public void robotInit() { motorBulkActions.setNeutralModeBulk(Arrays.asList( robotContainer.shooterSubsystem.shooterPitchMotor, - robotContainer.shooterSubsystem.shooterMotor, robotContainer.climberSubsystem.climberLeft, robotContainer.climberSubsystem.climberRight ), NeutralModeValue.Brake); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 840b678b..c045e4f1 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -246,6 +246,23 @@ public Rotation2d getAngleToHub() { return Rotation2d.fromDegrees(rotationAngleDegrees); } + public double getDistanceToHub() { + Pose2d robotPose = getPose2d(); + + Translation2d targetHub = DriverStation.getAlliance() + .orElse(Alliance.Blue) + .equals(Alliance.Blue) + ? DriveConstants.HUB_BLUE_POSITION: + DriveConstants.HUB_RED_POSITION; + + // Get X distance + double xDistance = targetHub.getX() - robotPose.getX(); + // Get Y distance + double yDistance = targetHub.getY() - robotPose.getY(); + + return Math.sqrt((xDistance * xDistance) + (yDistance * yDistance)); + } + public static Pose2d flipAlliance(Pose2d pose) { if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red) { return new Pose2d( diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index c5fd9e42..aa02d498 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -6,14 +6,14 @@ import com.btwrobotics.WhatTime.frc.MotorManagers.MotorWrapper; import com.btwrobotics.WhatTime.frc.MotorManagers.PositionManager; -import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeSubsystem extends SubsystemBase { - private double minValue = 0.0; - private double maxValue = 1.0; + private final double minValue = 0.0; + private final double maxValue = 1.0; + private final double threshold = 0.2; private List angleMotors = List.of( new MotorWrapper( @@ -23,14 +23,14 @@ public class IntakeSubsystem extends SubsystemBase { @Override public void periodic() { - Logger.recordOutput("IntakeSubsystem/Angle", angleEncoder.getAbsolutePosition().getValueAsDouble()); + Logger.recordOutput("IntakeSubsystem/Angle", angleMotors.get(0).getPosition()); } private MotorWrapper intakeWheelsMotor = new MotorWrapper( new TalonFX(10), false ); - private CANcoder angleEncoder = new CANcoder(0); + // private CANcoder angleEncoder = new CANcoder(35); private PositionManager intakePositionManager = new PositionManager( minValue, @@ -38,13 +38,14 @@ public void periodic() { angleMotors, 0.2, 0.0, + threshold, 0.02, - () -> angleEncoder.getAbsolutePosition().getValueAsDouble() + () -> intakeWheelsMotor.getPosition() // Use motor encoder for position ); public void setPosition(double targetPosition) { Logger.recordOutput("IntakeSubsystem/SetPosition", targetPosition); - intakePositionManager.move(targetPosition); + intakePositionManager.setTarget(targetPosition); } private double intakeSpeed = 0.5; diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterDataPoint.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterDataPoint.java new file mode 100644 index 00000000..124d1d14 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterDataPoint.java @@ -0,0 +1,17 @@ +package frc.robot.subsystems.mechanisms.shooter; + +public class ShooterDataPoint { + public double distance; + public double angle; + public double speed; + + public ShooterDataPoint( + double distance, + double angle, + double speed + ) { + this.distance = distance; + this.angle = angle; + this.speed = speed; + } +} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 54276f6c..ea89ffa4 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -1,7 +1,10 @@ package frc.robot.subsystems.mechanisms.shooter; import java.util.List; +import java.util.Map; +import java.util.TreeMap; +import com.btwrobotics.WhatTime.frc.FlywheelPair; import com.btwrobotics.WhatTime.frc.MotorManagers.MotorWrapper; import com.btwrobotics.WhatTime.frc.MotorManagers.PositionManager; import com.ctre.phoenix6.hardware.Pigeon2; @@ -32,28 +35,40 @@ public class ShooterSubsystem extends SubsystemBase { /** Reference to the drivetrain for coordinated aiming. */ Drive drivetrain; + TreeMap dataPoints = new TreeMap<>(); + + List shooterDataPoints = List.of( + // TODO: Collect successful data points and add them here + new ShooterDataPoint(0, 0, 0) + ); + /** * Constructs the ShooterSubsystem. * @param drivetrain the swerve drivetrain subsystem (for aiming/coordination) */ public ShooterSubsystem(Drive drivetrain) { this.drivetrain = drivetrain; + + for (ShooterDataPoint point : shooterDataPoints) { + dataPoints.put(point.distance, point); + } } /** Utility for motion calculations (e.g., trajectory, angles). */ MotorSubsystem motorSubsystem = new MotorSubsystem(); - /** Motor controlling the shooter flywheel. */ - public final MotorWrapper shooterMotor = new MotorWrapper( - new TalonFX(10), - false - ); /** Motor controlling the shooter pitch (angle). */ public final MotorWrapper shooterPitchMotor = new MotorWrapper( - new TalonFX(9), + new TalonFX(11), false ); + public FlywheelPair shooterMotors = new FlywheelPair( + new MotorWrapper(new TalonFX(12), false), // Left shooter motor + new MotorWrapper(new TalonFX(13), true), // Right shooter motor + 0.4 + ); + /** IMU sensor for shooter orientation feedback. */ public final Pigeon2 shooterPigeon = new Pigeon2(34); @@ -90,6 +105,7 @@ public ShooterSubsystem(Drive drivetrain) { List.of(shooterPitchMotor), 0.2, 0.0, + 0.05, positionThreshold, () -> getShooterPitchDeg() ); @@ -99,10 +115,9 @@ public ShooterSubsystem(Drive drivetrain) { /** * Returns a command to pitch the shooter to the specified angle (degrees). * @param angle target pitch angle in degrees - * @return a command that moves the shooter pitch to the given angle */ - public Command pitchToAngleDeg(double angle) { - return shooterPositionManager.move(angle); + public void pitchToAngleDeg(double angle) { + shooterPositionManager.setTarget(angle); } /** @@ -114,14 +129,13 @@ public Command pitchToAngleDeg(double angle) { public Command aimAtHub() { return Commands.run( () -> { - // Get robot position (Should be implemented into WhatTime) - // Calculate distance to hub (Should be implemented into WhatTime) - - // Calculate required rotations to face hub - // Rotate to face hub + // Calculate the angle and speed needed + ShooterDataPoint hubCalculateDataPoint = calculateShooterValues( + shooterUpperLower(), + drivetrain.getDistanceToHub() + ); - // Calculate required shooter pitch angle to hit hub - // Pitch shooter to required angle + pitchToAngleDeg(hubCalculateDataPoint.angle); } ); } @@ -141,8 +155,31 @@ public double getShooterMotorPitchDeg() { * @return shooter pitch in degrees (roll axis) */ public double getShooterPitchDeg() { - // MARK: Should be roll? return shooterPigeon.getRoll().getValueAsDouble(); } + public UpperLowerPoint shooterUpperLower() { + double currentDistance = drivetrain.getDistanceToHub(); + Map.Entry lowerEntry = dataPoints.floorEntry(currentDistance); + Map.Entry upperEntry = dataPoints.ceilingEntry(currentDistance); + + // Handle out-of-range cases by clamping to the nearest point + ShooterDataPoint lower = (lowerEntry != null) ? lowerEntry.getValue() : upperEntry.getValue(); + ShooterDataPoint upper = (upperEntry != null) ? upperEntry.getValue() : lowerEntry.getValue(); + + return new UpperLowerPoint(upper, lower); + } + + public ShooterDataPoint calculateShooterValues(UpperLowerPoint upperLowerPoint, double currentDistance) { + double valueRange = Math.abs(upperLowerPoint.getUpperDistance() - upperLowerPoint.getLowerDistance()); + double scaledValue = currentDistance - Math.min(upperLowerPoint.getUpperDistance(), upperLowerPoint.getLowerDistance()); + + double interpolationFactor = scaledValue/valueRange; + + // Interpolate the angle and speed between the data points + double estimatedAngle = upperLowerPoint.getLowerAngle() + interpolationFactor * (upperLowerPoint.getUpperAngle() - upperLowerPoint.getLowerAngle()); + double estimatedSpeed = upperLowerPoint.getLowerSpeed() + interpolationFactor * (upperLowerPoint.getUpperSpeed() - upperLowerPoint.getLowerSpeed()); + + return new ShooterDataPoint(currentDistance, estimatedAngle, estimatedSpeed); + } } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/UpperLowerPoint.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/UpperLowerPoint.java new file mode 100644 index 00000000..db28d3c6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/UpperLowerPoint.java @@ -0,0 +1,38 @@ +package frc.robot.subsystems.mechanisms.shooter; + +public class UpperLowerPoint { + public ShooterDataPoint upper; + public ShooterDataPoint lower; + + public UpperLowerPoint( + ShooterDataPoint upper, + ShooterDataPoint lower + ) { + this.upper = upper; + this.lower = lower; + } + + public double getUpperDistance() { + return upper.distance; + } + + public double getLowerDistance() { + return lower.distance; + } + + public double getUpperAngle() { + return upper.angle; + } + + public double getLowerAngle() { + return lower.angle; + } + + public double getUpperSpeed() { + return upper.speed; + } + + public double getLowerSpeed() { + return lower.speed; + } +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 9e9152b0..868ae9d3 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,35 +1,35 @@ { - "fileName": "AdvantageKit.json", - "name": "AdvantageKit", - "version": "26.0.0", - "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "frcYear": "2026", - "mavenUrls": [ - "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" - ], - "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", - "javaDependencies": [ - { - "groupId": "org.littletonrobotics.akit", - "artifactId": "akit-java", - "version": "26.0.0" - } - ], - "jniDependencies": [ - { - "groupId": "org.littletonrobotics.akit", - "artifactId": "akit-wpilibio", - "version": "26.0.0", - "skipInvalidPlatforms": false, - "isJar": false, - "validPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ], - "cppDependencies": [] + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "26.0.1", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2026", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "26.0.1" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "26.0.1", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] } \ No newline at end of file diff --git a/vendordeps/WhatTime.json b/vendordeps/WhatTime.json index 7f68d20d..54fcac61 100644 --- a/vendordeps/WhatTime.json +++ b/vendordeps/WhatTime.json @@ -1,7 +1,7 @@ { "fileName": "WhatTime.json", "name": "WhatTime", - "version": "v2026.1.16-beta.1", + "version": "v2026.1.16-beta.3", "frcYear": 2026, "uuid": "2C0B26E9-47AA-4C55-B235-640C45ECAD33", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.btwrobotics.whattime.frc", "artifactId": "whattime", - "version": "v2026.1.16-beta.1" + "version": "v2026.1.16-beta.3" } ], "jniDependencies": [], From 3f90d5e0f356ce86bbb2d4516672d77aed12a9e8 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Thu, 26 Feb 2026 01:28:52 -0600 Subject: [PATCH 002/102] Made more changes to shooter subsystem. idk --- src/main/java/frc/robot/BuildConstants.java | 10 +++--- .../frc/robot/subsystems/drive/Drive.java | 1 - .../mechanisms/shooter/ShooterSubsystem.java | 31 ++++++++++++------- 3 files changed, 25 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index eccfb282..35fbd484 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026 Roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 125; - public static final String GIT_SHA = "2b20aba8b0841b8d59dc828f7f6cfadd181b217d"; - public static final String GIT_DATE = "2026-02-25 19:31:48 CST"; + public static final int GIT_REVISION = 126; + public static final String GIT_SHA = "b4a0f573db2ebc849d38b58f5d5485d5f6626516"; + public static final String GIT_DATE = "2026-02-26 01:19:08 CST"; public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-02-26 01:17:31 CST"; - public static final long BUILD_UNIX_TIME = 1772090251769L; + public static final String BUILD_DATE = "2026-02-26 01:27:15 CST"; + public static final long BUILD_UNIX_TIME = 1772090835593L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index c045e4f1..21c0b03d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -26,7 +26,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index ea89ffa4..8a3c06f7 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -53,9 +53,6 @@ public ShooterSubsystem(Drive drivetrain) { dataPoints.put(point.distance, point); } } - - /** Utility for motion calculations (e.g., trajectory, angles). */ - MotorSubsystem motorSubsystem = new MotorSubsystem(); /** Motor controlling the shooter pitch (angle). */ public final MotorWrapper shooterPitchMotor = new MotorWrapper( @@ -75,12 +72,12 @@ public ShooterSubsystem(Drive drivetrain) { // --- Shooter configuration and tuning fields --- /** Entry angle to the hub in degrees (TODO: calculate actual value). */ - public double hubEnterAngle = -70; + // public double hubEnterAngle = -70; /** Speed for pitching the shooter (open-loop, 0..1). */ - public double shooterPitchSpeed = 0.1; + // public double shooterPitchSpeed = 0.1; /** Hold speed for maintaining shooter pitch (open-loop, 0..1). */ - public double shooterPitchHoldSpeed = 0.02; + public double shooterPitchHoldSpeed = 0.0; /** Maximum allowed shooter pitch (units depend on mechanism, e.g., rotations or percent). */ public double shooterPitchMax = 0.3; @@ -91,9 +88,9 @@ public ShooterSubsystem(Drive drivetrain) { public double positionThreshold = 1.0; /** Height of the hub (target) in meters. */ - public double hubHeight = 2; + // public double hubHeight = 2; /** Height of the shooter in meters. */ - public double shooterHeight = 1; + // public double shooterHeight = 1; /** * PositionManager for controlling the shooter pitch motor to a target angle. @@ -104,9 +101,9 @@ public ShooterSubsystem(Drive drivetrain) { shooterPitchMax, List.of(shooterPitchMotor), 0.2, - 0.0, - 0.05, - positionThreshold, + shooterPitchHoldSpeed, + positionThreshold, + 0.05, () -> getShooterPitchDeg() ); @@ -136,6 +133,8 @@ public Command aimAtHub() { ); pitchToAngleDeg(hubCalculateDataPoint.angle); + + shooterMotors.setSpeed(hubCalculateDataPoint.speed); } ); } @@ -159,6 +158,12 @@ public double getShooterPitchDeg() { } public UpperLowerPoint shooterUpperLower() { + if (dataPoints.isEmpty()) { + return new UpperLowerPoint( + new ShooterDataPoint(0, 0, 0), + new ShooterDataPoint(0, 0, 0) + ); + } double currentDistance = drivetrain.getDistanceToHub(); Map.Entry lowerEntry = dataPoints.floorEntry(currentDistance); Map.Entry upperEntry = dataPoints.ceilingEntry(currentDistance); @@ -172,6 +177,10 @@ public UpperLowerPoint shooterUpperLower() { public ShooterDataPoint calculateShooterValues(UpperLowerPoint upperLowerPoint, double currentDistance) { double valueRange = Math.abs(upperLowerPoint.getUpperDistance() - upperLowerPoint.getLowerDistance()); + if (valueRange == 0) { + return new ShooterDataPoint(currentDistance, upperLowerPoint.getUpperAngle(), upperLowerPoint.getUpperSpeed()); + } + double scaledValue = currentDistance - Math.min(upperLowerPoint.getUpperDistance(), upperLowerPoint.getLowerDistance()); double interpolationFactor = scaledValue/valueRange; From 3e4f7b22ce9cab78400c4af135a0eb5043b78547 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Thu, 26 Feb 2026 15:56:29 -0600 Subject: [PATCH 003/102] Removed debug commands from driver joysticks. Added them to debug joystick. --- .../namedcommands/RegisterCommands.java | 9 +++-- .../frc/robot/joysticks/DebugJoystick.java | 26 ++++++++++++--- .../frc/robot/joysticks/DriverJoystick.java | 33 ++----------------- 3 files changed, 32 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java index a5cf3484..53bef77b 100644 --- a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java +++ b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java @@ -13,7 +13,12 @@ public class RegisterCommands { ClimberSubsystem climberSubsystem; MotorSubsystem motorSubsystem; - public RegisterCommands(IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem, ClimberSubsystem climberSubsystem, MotorSubsystem motorSubsystem) { + public RegisterCommands( + IntakeSubsystem intakeSubsystem, + ShooterSubsystem shooterSubsystem, + ClimberSubsystem climberSubsystem, + MotorSubsystem motorSubsystem + ) { this.intakeSubsystem = intakeSubsystem; this.shooterSubsystem = shooterSubsystem; this.climberSubsystem = climberSubsystem; @@ -21,7 +26,7 @@ public RegisterCommands(IntakeSubsystem intakeSubsystem, ShooterSubsystem shoote } public void registerCommands(){ - NamedCommands.registerCommand("shoot", shooterSubsystem.aimAtHub()); + NamedCommands.registerCommand("AngleToHub", shooterSubsystem.aimAtHub()); } } diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 3ba896cd..3d1d3707 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -2,9 +2,11 @@ import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.vision.limelight.LimelightConstants; import frc.robot.subsystems.vision.limelight.LimelightHelpers; public class DebugJoystick { @@ -36,18 +38,34 @@ public void configureBindings() { joystick.leftBumper(); - joystick.povUp(); + // Reset pose to limelight output + joystick.povUp().onTrue( + Commands.runOnce( + () -> { + Logger.recordOutput("SwerveDrive/SetSwervePoseLimelight", true); + + drivetrain.resetPose(LimelightHelpers.getBotPose2d("limelight-four")); + } + ) + ); // Reset Field Centric Heading - joystick.povDown().onTrue(drivetrain.runOnce(drivetrain.drivetrain::seedFieldCentric)); + joystick.povDown().onTrue( + drivetrain.runOnce(drivetrain.drivetrain::seedFieldCentric) + ); joystick.povLeft().onTrue( Commands.runOnce( () -> { Logger.recordOutput("QuestNav/SetQuestPose", true); // Reset QuestNav pose to Limelight position - drivetrain.questNavSubsystem.setQuestPose(LimelightHelpers.getBotPose3d_wpiBlue("limelight-four")); - + drivetrain.questNavSubsystem.setQuestPose( + LimelightHelpers.getBotPose3d_wpiBlue("limelight-four") + .transformBy( + new Transform3d(LimelightConstants.LIMELIGHT_4_TRANSFORM_FROM_CENTRE).inverse() + ) + ); + Logger.recordOutput("QuestNav/SetQuestPose", false); } ) diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index 7e8d2743..9bdd309b 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -46,38 +46,11 @@ public void configureBindings() { joystick.leftBumper(); - // Reset pose to limelight output - joystick.povUp().onTrue( - Commands.runOnce( - () -> { - Logger.recordOutput("SwerveDrive/SetSwervePoseLimelight", true); - - drivetrain.resetPose(LimelightHelpers.getBotPose2d("limelight-four")); - } - ) - ); + joystick.povUp(); - // Reset Field Centric Heading - joystick.povDown().onTrue(drivetrain.runOnce(drivetrain.drivetrain::seedFieldCentric)); + joystick.povDown(); - //joystick.povDown(); - - joystick.povLeft().onTrue( - Commands.runOnce( - () -> { - Logger.recordOutput("QuestNav/SetQuestPose", true); - // Reset QuestNav pose to Limelight position - drivetrain.questNavSubsystem.setQuestPose( - LimelightHelpers.getBotPose3d_wpiBlue("limelight-four") - .transformBy( - new Transform3d(LimelightConstants.LIMELIGHT_4_TRANSFORM_FROM_CENTRE).inverse() - ) - ); - - Logger.recordOutput("QuestNav/SetQuestPose", false); - } - ) - ); + joystick.povLeft(); joystick.povRight(); } From 645e6bfbe09aa24891344f78a541ca2e90efcba0 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Thu, 26 Feb 2026 16:09:59 -0600 Subject: [PATCH 004/102] More small updates to stuff --- src/main/java/frc/robot/subsystems/drive/Drive.java | 8 ++++++++ .../mechanisms/shooter/ShooterSubsystem.java | 10 +++++++--- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 21c0b03d..b848a779 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -84,6 +84,8 @@ public Drive(CommandSwerveDrivetrain drivetrain) { private boolean lockedToHub = false; + public boolean debugMode = false; + // MARK: Periodic Loop @Override public void periodic() { @@ -98,6 +100,8 @@ public void periodic() { Logger.recordOutput("SwerveDrive/Rotation", getPose2d().getRotation()); Logger.recordOutput("SwerveDrive/TargetHubAngle", getAngleToHub()); + + Logger.recordOutput("Debug/Enabled", debugMode); } public Command applyRequest(Supplier request) { @@ -272,4 +276,8 @@ public static Pose2d flipAlliance(Pose2d pose) { } return pose; } + + public void toggleDebugMode() { + debugMode = !debugMode; + } } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 8a3c06f7..32bbaa83 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.motor.MotorSubsystem; @@ -61,11 +60,16 @@ public ShooterSubsystem(Drive drivetrain) { ); public FlywheelPair shooterMotors = new FlywheelPair( - new MotorWrapper(new TalonFX(12), false), // Left shooter motor - new MotorWrapper(new TalonFX(13), true), // Right shooter motor + new MotorWrapper(new TalonFX(13), false), // Left shooter motor + new MotorWrapper(new TalonFX(14), true), // Right shooter motor 0.4 ); + public MotorWrapper feedMotor = new MotorWrapper( + new TalonFX(12), + false + ); + /** IMU sensor for shooter orientation feedback. */ public final Pigeon2 shooterPigeon = new Pigeon2(34); From 75ea30f1f9751ca1068cc1e782d8da7900c8e470 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Thu, 26 Feb 2026 17:07:05 -0600 Subject: [PATCH 005/102] Added a bunch of autos because why not. --- .../autos/{Tests.auto => 1- HOH.auto} | 12 +- .../deploy/pathplanner/autos/2 - HDH.auto | 31 ++++ .../deploy/pathplanner/autos/3- HDHOH.auto | 31 ++++ .../pathplanner/paths/HDH - Part 1.path | 37 +++-- .../pathplanner/paths/HDH - Part 2.path | 49 ++++-- .../pathplanner/paths/HDH - Part 3.path | 37 +++-- .../pathplanner/paths/HDHOH - Part 1.path | 86 +++++++++++ .../pathplanner/paths/HDHOH - Part 2.path | 111 ++++++++++++++ .../pathplanner/paths/HDHOH - Part 3.path | 65 ++++++++ .../pathplanner/paths/HOH - Part 1.path | 65 ++++++++ .../pathplanner/paths/HOH - Part 2.path | 54 +++++++ .../pathplanner/paths/HOH - Part 3.path | 65 ++++++++ .../deploy/pathplanner/paths/New Path.path | 91 ------------ .../deploy/pathplanner/paths/SimplePath.path | 139 ------------------ src/main/deploy/pathplanner/settings.json | 4 +- 15 files changed, 603 insertions(+), 274 deletions(-) rename src/main/deploy/pathplanner/autos/{Tests.auto => 1- HOH.auto} (58%) create mode 100644 src/main/deploy/pathplanner/autos/2 - HDH.auto create mode 100644 src/main/deploy/pathplanner/autos/3- HDHOH.auto create mode 100644 src/main/deploy/pathplanner/paths/HDHOH - Part 1.path create mode 100644 src/main/deploy/pathplanner/paths/HDHOH - Part 2.path create mode 100644 src/main/deploy/pathplanner/paths/HDHOH - Part 3.path create mode 100644 src/main/deploy/pathplanner/paths/HOH - Part 1.path create mode 100644 src/main/deploy/pathplanner/paths/HOH - Part 2.path create mode 100644 src/main/deploy/pathplanner/paths/HOH - Part 3.path delete mode 100644 src/main/deploy/pathplanner/paths/New Path.path delete mode 100644 src/main/deploy/pathplanner/paths/SimplePath.path diff --git a/src/main/deploy/pathplanner/autos/Tests.auto b/src/main/deploy/pathplanner/autos/1- HOH.auto similarity index 58% rename from src/main/deploy/pathplanner/autos/Tests.auto rename to src/main/deploy/pathplanner/autos/1- HOH.auto index 15fbbbda..c95f0ecb 100644 --- a/src/main/deploy/pathplanner/autos/Tests.auto +++ b/src/main/deploy/pathplanner/autos/1- HOH.auto @@ -7,13 +7,19 @@ { "type": "path", "data": { - "pathName": "SimplePath" + "pathName": "HOH - Part 1" } }, { - "type": "named", + "type": "path", + "data": { + "pathName": "HOH - Part 2" + } + }, + { + "type": "path", "data": { - "name": "shoot" + "pathName": "HOH - Part 3" } } ] diff --git a/src/main/deploy/pathplanner/autos/2 - HDH.auto b/src/main/deploy/pathplanner/autos/2 - HDH.auto new file mode 100644 index 00000000..92b1a6de --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 - HDH.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "HDH - Part 1" + } + }, + { + "type": "path", + "data": { + "pathName": "HDH - Part 2" + } + }, + { + "type": "path", + "data": { + "pathName": "HDH - Part 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3- HDHOH.auto b/src/main/deploy/pathplanner/autos/3- HDHOH.auto new file mode 100644 index 00000000..c6b0ddff --- /dev/null +++ b/src/main/deploy/pathplanner/autos/3- HDHOH.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "HDHOH - Part 1" + } + }, + { + "type": "path", + "data": { + "pathName": "HDHOH - Part 2" + } + }, + { + "type": "path", + "data": { + "pathName": "HDHOH - Part 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 1.path b/src/main/deploy/pathplanner/paths/HDH - Part 1.path index e40a25de..1e4cb227 100644 --- a/src/main/deploy/pathplanner/paths/HDH - Part 1.path +++ b/src/main/deploy/pathplanner/paths/HDH - Part 1.path @@ -3,34 +3,45 @@ "waypoints": [ { "anchor": { - "x": 3.6584026786452846, - "y": 3.816648148148148 + "x": 3.666775798905367, + "y": 6.3921116922669485 }, "prevControl": null, "nextControl": { - "x": 2.6584026786452846, - "y": 3.816648148148148 + "x": 3.1429084260122266, + "y": 6.649324898279437 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.495586630003213, - "y": 3.816648148148148 + "x": 1.7694349179025424, + "y": 6.753512259445622 }, "prevControl": { - "x": 3.495586630003213, - "y": 3.816648148148148 + "x": 2.6898677817295176, + "y": 6.031772232809059 }, "nextControl": null, "isLocked": false, - "linkedName": "HDH Point 1" + "linkedName": "HDH - 1" } ], "rotationTargets": [], "constraintZones": [], - "pointTowardsZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.626, + "y": 4.034 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.6040311706852791, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -42,13 +53,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": -43.89633519981335 }, "reversed": false, - "folder": "1 - Hub, Depot, Hub (HDH)", + "folder": "2 - Hub, Depot, Hub (HDH)", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 2.path b/src/main/deploy/pathplanner/paths/HDH - Part 2.path index efd5399d..fbb86441 100644 --- a/src/main/deploy/pathplanner/paths/HDH - Part 2.path +++ b/src/main/deploy/pathplanner/paths/HDH - Part 2.path @@ -3,32 +3,53 @@ "waypoints": [ { "anchor": { - "x": 2.495586630003213, - "y": 3.816648148148148 + "x": 1.7694349179025424, + "y": 6.753512259445622 }, "prevControl": null, "nextControl": { - "x": 2.495586630003213, - "y": 1.8166481481481478 + "x": 1.6072378556769904, + "y": 7.415759675142259 }, "isLocked": false, - "linkedName": "HDH Point 1" + "linkedName": "HDH - 1" }, { "anchor": { - "x": 0.37219777199074133, - "y": 0.6346029369212964 + "x": 0.40981588762358767, + "y": 6.858705033986582 }, "prevControl": { - "x": 2.3721977719907414, - "y": 0.6346029369212967 + "x": 0.44795100584885555, + "y": 7.105779340133425 + }, + "nextControl": { + "x": 0.34899375441384184, + "y": 6.464643416754943 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.40981588762358767, + "y": 5.010476694915254 + }, + "prevControl": { + "x": 0.40981588762358767, + "y": 5.3825688815658905 }, "nextControl": null, "isLocked": false, - "linkedName": "HDH Point 2" + "linkedName": "HDH - 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0, + "rotationDegrees": -90.0 } ], - "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -42,13 +63,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": -89.62181552849039 }, "reversed": false, - "folder": "1 - Hub, Depot, Hub (HDH)", + "folder": "2 - Hub, Depot, Hub (HDH)", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -43.89633519981335 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 3.path b/src/main/deploy/pathplanner/paths/HDH - Part 3.path index b349155b..fa49e557 100644 --- a/src/main/deploy/pathplanner/paths/HDH - Part 3.path +++ b/src/main/deploy/pathplanner/paths/HDH - Part 3.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 0.37219777199074133, - "y": 0.6346029369212964 + "x": 0.40981588762358767, + "y": 5.010476694915254 }, "prevControl": null, "nextControl": { - "x": 1.3721977719907414, - "y": 0.6346029369212962 + "x": 1.3816289629778247, + "y": 4.955293945190116 }, "isLocked": false, - "linkedName": "HDH Point 2" + "linkedName": "HDH - 2" }, { "anchor": { - "x": 2.4603841869212966, - "y": 2.5775276331018517 + "x": 2.276431188206215, + "y": 6.445772587835452 }, "prevControl": { - "x": 2.4603841869212966, - "y": 1.654830760080282 + "x": 1.329324591150149, + "y": 5.973136390129629 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,18 @@ ], "rotationTargets": [], "constraintZones": [], - "pointTowardsZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.626, + "y": 4.034 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.36584906408629436, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, @@ -42,13 +53,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 34.63187389435466 + "rotation": -48.676167316772535 }, "reversed": false, - "folder": "1 - Hub, Depot, Hub (HDH)", + "folder": "2 - Hub, Depot, Hub (HDH)", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -89.62181552849039 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path b/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path new file mode 100644 index 00000000..038521d3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6901465947210452, + "y": 6.353015205685028 + }, + "prevControl": null, + "nextControl": { + "x": 1.9768597288320737, + "y": 5.994681095862107 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.5577825410487289, + "y": 5.175910961776131 + }, + "prevControl": { + "x": 0.8534255274540962, + "y": 4.392190920727401 + }, + "nextControl": { + "x": 0.4462496135096857, + "y": 5.471573617643542 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.3952998653778249, + "y": 6.7493993864759885 + }, + "prevControl": { + "x": 0.39529986537782486, + "y": 5.589795892453111 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HDHOH - 1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9785940803382666, + "rotationDegrees": -78.6156485289686 + } + ], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.626, + "y": 4.034 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.1, + "maxWaypointRelativePos": 0.7675682106598984, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "3 - Hub, Depot, Hub, Outpost, Hub (HDHOH)", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path b/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path new file mode 100644 index 00000000..94499535 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.3952998653778249, + "y": 6.7493993864759885 + }, + "prevControl": null, + "nextControl": { + "x": 1.5322086464191353, + "y": 7.019345013370341 + }, + "isLocked": false, + "linkedName": "HDHOH - 1" + }, + { + "anchor": { + "x": 2.06691660045904, + "y": 4.663688923463983 + }, + "prevControl": { + "x": 2.013865287368215, + "y": 5.407969223567639 + }, + "nextControl": { + "x": 2.114577540165961, + "y": 3.995032552083333 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5759363413665257, + "y": 2.3252545131532485 + }, + "prevControl": { + "x": 1.6788570280910506, + "y": 2.589121599919883 + }, + "nextControl": { + "x": 1.4843147081995867, + "y": 2.0903558334226107 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.3866386387711864, + "y": 0.703621281338277 + }, + "prevControl": { + "x": 0.6692790215537343, + "y": 1.7680030816735008 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HDHOH - 2" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.5793345494923854, + "maxWaypointRelativePos": 2.216301157994923, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.626, + "y": 4.034 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.6, + "maxWaypointRelativePos": 2.2081218274111687, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 88.67957408826696 + }, + "reversed": false, + "folder": "3 - Hub, Depot, Hub, Outpost, Hub (HDHOH)", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDHOH - Part 3.path b/src/main/deploy/pathplanner/paths/HDHOH - Part 3.path new file mode 100644 index 00000000..72c78a6b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HDHOH - Part 3.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.3866386387711864, + "y": 0.703621281338277 + }, + "prevControl": null, + "nextControl": { + "x": 0.930785889823216, + "y": 1.477687215900453 + }, + "isLocked": false, + "linkedName": "HDHOH - 2" + }, + { + "anchor": { + "x": 2.0376910090042375, + "y": 3.9501296566031074 + }, + "prevControl": { + "x": 2.2442816429688675, + "y": 3.028219417632113 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.626, + "y": 4.034 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.25, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "3 - Hub, Depot, Hub, Outpost, Hub (HDHOH)", + "idealStartingState": { + "velocity": 0, + "rotation": 88.67957408826696 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HOH - Part 1.path b/src/main/deploy/pathplanner/paths/HOH - Part 1.path new file mode 100644 index 00000000..a63d2c97 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HOH - Part 1.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6584026786452846, + "y": 3.816648148148148 + }, + "prevControl": null, + "nextControl": { + "x": 2.6584026786452846, + "y": 3.816648148148148 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.495586630003213, + "y": 3.816648148148148 + }, + "prevControl": { + "x": 3.495586630003213, + "y": 3.816648148148148 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HOH - 1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.626, + "y": 4.034 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 4.162462895863767 + }, + "reversed": false, + "folder": "1 - Hub, Outpost, Hub (HOH)", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HOH - Part 2.path b/src/main/deploy/pathplanner/paths/HOH - Part 2.path new file mode 100644 index 00000000..26ffe15f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HOH - Part 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.495586630003213, + "y": 3.816648148148148 + }, + "prevControl": null, + "nextControl": { + "x": 2.495586630003213, + "y": 1.8166481481481478 + }, + "isLocked": false, + "linkedName": "HOH - 1" + }, + { + "anchor": { + "x": 0.37219777199074133, + "y": 0.6346029369212964 + }, + "prevControl": { + "x": 2.3721977719907414, + "y": 0.6346029369212967 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HOH - 2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "1 - Hub, Outpost, Hub (HOH)", + "idealStartingState": { + "velocity": 0, + "rotation": 4.162462895863767 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HOH - Part 3.path b/src/main/deploy/pathplanner/paths/HOH - Part 3.path new file mode 100644 index 00000000..7c2b4f42 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HOH - Part 3.path @@ -0,0 +1,65 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.37219777199074133, + "y": 0.6346029369212964 + }, + "prevControl": null, + "nextControl": { + "x": 1.5086383595418487, + "y": 0.8460493881849196 + }, + "isLocked": false, + "linkedName": "HOH - 2" + }, + { + "anchor": { + "x": 2.4603841869212966, + "y": 2.5775276331018517 + }, + "prevControl": { + "x": 2.086335770806592, + "y": 2.3306133708545804 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.626, + "y": 4.034 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.4377082011421318, + "maxWaypointRelativePos": 1.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 34.63187389435466 + }, + "reversed": false, + "folder": "1 - Hub, Outpost, Hub (HOH)", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path deleted file mode 100644 index a4f1409a..00000000 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ /dev/null @@ -1,91 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.646, - "y": 3.839 - }, - "prevControl": null, - "nextControl": { - "x": 3.6659141248056653, - "y": 2.7709942982324054 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.646, - "y": 2.5083085214120375 - }, - "prevControl": { - "x": 3.5675651685926173, - "y": 2.745685810178456 - }, - "nextControl": { - "x": 3.757652405960003, - "y": 2.1704006693077176 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.874690682870371, - "y": 2.273750868055556 - }, - "prevControl": { - "x": 5.628975714164271, - "y": 2.2276623211728372 - }, - "nextControl": { - "x": 6.12040565157647, - "y": 2.319839414938275 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.42375535300926, - "y": 2.273750868055556 - }, - "prevControl": { - "x": 6.274127604166668, - "y": 2.0338131510416666 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.2, - "rotationDegrees": 45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 45.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SimplePath.path b/src/main/deploy/pathplanner/paths/SimplePath.path deleted file mode 100644 index 27e551fa..00000000 --- a/src/main/deploy/pathplanner/paths/SimplePath.path +++ /dev/null @@ -1,139 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6078573019462716, - "y": 1.9760754180372806 - }, - "prevControl": null, - "nextControl": { - "x": 3.542609194557422, - "y": 2.2174106298945087 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.632999802974233, - "y": 2.532426800644188 - }, - "prevControl": { - "x": 3.637278791529922, - "y": 1.5328750011405277 - }, - "nextControl": { - "x": 5.499249744303021, - "y": 3.4020094224060635 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.001672106291119, - "y": 3.4112223307291663 - }, - "prevControl": { - "x": 6.599926789143243, - "y": 3.405055521175869 - }, - "nextControl": { - "x": 7.542175118963073, - "y": 3.4195190774486917 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.71500089946546, - "y": 2.532426800644188 - }, - "prevControl": { - "x": 7.8853493729799276, - "y": 3.1124078786697877 - }, - "nextControl": { - "x": 7.462741885480951, - "y": 1.6735670066350252 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.9750896038925445, - "y": 2.2326333778782894 - }, - "prevControl": { - "x": 5.216321610053111, - "y": 2.16700473166531 - }, - "nextControl": { - "x": 3.946295606997631, - "y": 2.5125230920288746 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.483472322162829, - "y": 2.9627905701754376 - }, - "prevControl": { - "x": 3.2772546423557425, - "y": 2.5587835655112747 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 1, - "rotationDegrees": -46.32240446147237 - }, - { - "waypointRelativePos": 1.5918582940251573, - "rotationDegrees": -18.50084926673444 - }, - { - "waypointRelativePos": 2.777736831761006, - "rotationDegrees": -89.11098055349674 - }, - { - "waypointRelativePos": 3.2713615369496862, - "rotationDegrees": -179.9430155787815 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3, - "maxAcceleration": 3, - "maxAngularVelocity": 540, - "maxAngularAcceleration": 720, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 31.038981760511703 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -0.18733647533512643 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 045f8583..f507046c 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -3,7 +3,9 @@ "robotLength": 0.762, "holonomicMode": true, "pathFolders": [ - "1 - Hub, Depot, Hub (HDH)" + "1 - Hub, Outpost, Hub (HOH)", + "2 - Hub, Depot, Hub (HDH)", + "3 - Hub, Depot, Hub, Outpost, Hub (HDHOH)" ], "autoFolders": [], "defaultMaxVel": 3.0, From 35221cd0d802b8bd923e88733fee4bee72f92344 Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Thu, 26 Feb 2026 17:36:03 -0600 Subject: [PATCH 006/102] More auto updates --- src/main/deploy/pathplanner/paths/HDH - Part 1.path | 13 +------------ src/main/deploy/pathplanner/paths/HDH - Part 2.path | 4 ++-- src/main/deploy/pathplanner/paths/HDH - Part 3.path | 2 +- .../deploy/pathplanner/paths/HDHOH - Part 1.path | 4 ++-- src/main/deploy/pathplanner/paths/HOH - Part 3.path | 2 +- src/main/java/frc/robot/BuildConstants.java | 12 ++++++------ src/main/java/frc/robot/RobotContainer.java | 11 ++++++++--- 7 files changed, 21 insertions(+), 27 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 1.path b/src/main/deploy/pathplanner/paths/HDH - Part 1.path index 1e4cb227..63ae8241 100644 --- a/src/main/deploy/pathplanner/paths/HDH - Part 1.path +++ b/src/main/deploy/pathplanner/paths/HDH - Part 1.path @@ -30,18 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "pointTowardsZones": [ - { - "fieldPosition": { - "x": 4.626, - "y": 4.034 - }, - "rotationOffset": 0.0, - "minWaypointRelativePos": 0.6040311706852791, - "maxWaypointRelativePos": 1.0, - "name": "Point Towards Zone" - } - ], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 2.path b/src/main/deploy/pathplanner/paths/HDH - Part 2.path index fbb86441..f969003a 100644 --- a/src/main/deploy/pathplanner/paths/HDH - Part 2.path +++ b/src/main/deploy/pathplanner/paths/HDH - Part 2.path @@ -20,8 +20,8 @@ "y": 6.858705033986582 }, "prevControl": { - "x": 0.44795100584885555, - "y": 7.105779340133425 + "x": 0.4479510058488559, + "y": 7.105779340133427 }, "nextControl": { "x": 0.34899375441384184, diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 3.path b/src/main/deploy/pathplanner/paths/HDH - Part 3.path index fa49e557..ec996598 100644 --- a/src/main/deploy/pathplanner/paths/HDH - Part 3.path +++ b/src/main/deploy/pathplanner/paths/HDH - Part 3.path @@ -20,7 +20,7 @@ "y": 6.445772587835452 }, "prevControl": { - "x": 1.329324591150149, + "x": 1.3293245911501488, "y": 5.973136390129629 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path b/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path index 038521d3..eef2b8b1 100644 --- a/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path +++ b/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path @@ -20,7 +20,7 @@ "y": 5.175910961776131 }, "prevControl": { - "x": 0.8534255274540962, + "x": 0.8534255274540963, "y": 4.392190920727401 }, "nextControl": { @@ -36,7 +36,7 @@ "y": 6.7493993864759885 }, "prevControl": { - "x": 0.39529986537782486, + "x": 0.39529986537782497, "y": 5.589795892453111 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/HOH - Part 3.path b/src/main/deploy/pathplanner/paths/HOH - Part 3.path index 7c2b4f42..0a7c583e 100644 --- a/src/main/deploy/pathplanner/paths/HOH - Part 3.path +++ b/src/main/deploy/pathplanner/paths/HOH - Part 3.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.5086383595418487, + "x": 1.5086383595418482, "y": 0.8460493881849196 }, "isLocked": false, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 35fbd484..ed46cb2b 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,14 +5,14 @@ */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026 Roomba"; + public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 126; - public static final String GIT_SHA = "b4a0f573db2ebc849d38b58f5d5485d5f6626516"; - public static final String GIT_DATE = "2026-02-26 01:19:08 CST"; + public static final int GIT_REVISION = 130; + public static final String GIT_SHA = "75ea30f1f9751ca1068cc1e782d8da7900c8e470"; + public static final String GIT_DATE = "2026-02-26 17:07:05 CST"; public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-02-26 01:27:15 CST"; - public static final long BUILD_UNIX_TIME = 1772090835593L; + public static final String BUILD_DATE = "2026-02-26 17:32:21 CST"; + public static final long BUILD_UNIX_TIME = 1772148741165L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fb7bde6d..256af0a6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; @@ -72,14 +74,16 @@ public class RobotContainer { // private final SendableChooser autoChooser; - private final LoggedDashboardChooser autoChooser; + private final SendableChooser autoChooser; public RobotContainer() { DriverStation.silenceJoystickConnectionWarning(true); registerCommands.registerCommands(); - autoChooser = new LoggedDashboardChooser<>("Autonomous Mode", AutoBuilder.buildAutoChooser()); + // autoChooser = new LoggedDashboardChooser<>("Autonomous Mode", AutoBuilder.buildAutoChooser()); + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); // MARK: Run Tests /* Disable tests on actual code */ @@ -131,6 +135,7 @@ private void configureBindings() { public Command getAutonomousCommand() { /* Run the path selected from the auto chooser */ - return autoChooser.get(); + // return autoChooser.get(); + return autoChooser.getSelected(); } } From 7dd0f7086a73997a5224878db97546d1a481f79f Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Thu, 26 Feb 2026 19:32:17 -0600 Subject: [PATCH 007/102] More adjustments for STD devs for vision. Updates to paths. --- src/main/deploy/pathplanner/paths/HDH - Part 1.path | 8 ++++---- src/main/deploy/pathplanner/paths/HDH - Part 2.path | 8 ++++---- src/main/deploy/pathplanner/paths/HDH - Part 3.path | 8 ++++---- src/main/deploy/pathplanner/paths/HDHOH - Part 1.path | 8 ++++---- src/main/deploy/pathplanner/paths/HDHOH - Part 2.path | 8 ++++---- src/main/deploy/pathplanner/paths/HDHOH - Part 3.path | 8 ++++---- src/main/deploy/pathplanner/paths/HOH - Part 1.path | 8 ++++---- src/main/deploy/pathplanner/paths/HOH - Part 2.path | 8 ++++---- src/main/deploy/pathplanner/paths/HOH - Part 3.path | 8 ++++---- .../deploy/pathplanner/paths/Testing Pose Path.path | 8 ++++---- src/main/deploy/pathplanner/settings.json | 8 ++++---- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/subsystems/drive/Drive.java | 2 +- .../vision/limelight/LimelightSubsystem.java | 4 ++-- .../subsystems/vision/questnav/QuestNavConstants.java | 6 +++--- 15 files changed, 55 insertions(+), 55 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 1.path b/src/main/deploy/pathplanner/paths/HDH - Part 1.path index 63ae8241..309e9cf1 100644 --- a/src/main/deploy/pathplanner/paths/HDH - Part 1.path +++ b/src/main/deploy/pathplanner/paths/HDH - Part 1.path @@ -33,10 +33,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 100.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 2.path b/src/main/deploy/pathplanner/paths/HDH - Part 2.path index f969003a..f8857c0c 100644 --- a/src/main/deploy/pathplanner/paths/HDH - Part 2.path +++ b/src/main/deploy/pathplanner/paths/HDH - Part 2.path @@ -54,10 +54,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 100.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 3.path b/src/main/deploy/pathplanner/paths/HDH - Part 3.path index ec996598..7d0bfdbb 100644 --- a/src/main/deploy/pathplanner/paths/HDH - Part 3.path +++ b/src/main/deploy/pathplanner/paths/HDH - Part 3.path @@ -44,10 +44,10 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 100.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path b/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path index eef2b8b1..6dd196af 100644 --- a/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path +++ b/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path @@ -65,10 +65,10 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 100.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path b/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path index 94499535..fcfcd5e7 100644 --- a/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path +++ b/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path @@ -90,10 +90,10 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 100.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/HDHOH - Part 3.path b/src/main/deploy/pathplanner/paths/HDHOH - Part 3.path index 72c78a6b..65b63199 100644 --- a/src/main/deploy/pathplanner/paths/HDHOH - Part 3.path +++ b/src/main/deploy/pathplanner/paths/HDHOH - Part 3.path @@ -44,10 +44,10 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 100.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/HOH - Part 1.path b/src/main/deploy/pathplanner/paths/HOH - Part 1.path index a63d2c97..3ca48fda 100644 --- a/src/main/deploy/pathplanner/paths/HOH - Part 1.path +++ b/src/main/deploy/pathplanner/paths/HOH - Part 1.path @@ -44,10 +44,10 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 100.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/HOH - Part 2.path b/src/main/deploy/pathplanner/paths/HOH - Part 2.path index 26ffe15f..f065b3ac 100644 --- a/src/main/deploy/pathplanner/paths/HOH - Part 2.path +++ b/src/main/deploy/pathplanner/paths/HOH - Part 2.path @@ -33,10 +33,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 100.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/HOH - Part 3.path b/src/main/deploy/pathplanner/paths/HOH - Part 3.path index 0a7c583e..8694c046 100644 --- a/src/main/deploy/pathplanner/paths/HOH - Part 3.path +++ b/src/main/deploy/pathplanner/paths/HOH - Part 3.path @@ -44,10 +44,10 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 100.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/Testing Pose Path.path b/src/main/deploy/pathplanner/paths/Testing Pose Path.path index c9ccdf50..14accaf4 100644 --- a/src/main/deploy/pathplanner/paths/Testing Pose Path.path +++ b/src/main/deploy/pathplanner/paths/Testing Pose Path.path @@ -33,10 +33,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 100.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index f507046c..920cc83e 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -8,10 +8,10 @@ "3 - Hub, Depot, Hub, Outpost, Hub (HDHOH)" ], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, - "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, + "defaultMaxVel": 1.5, + "defaultMaxAccel": 1.5, + "defaultMaxAngVel": 180.0, + "defaultMaxAngAccel": 100.0, "defaultNominalVoltage": 12.0, "robotMass": 75.0, "robotMOI": 6.883, diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index ed46cb2b..ef5446aa 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 130; - public static final String GIT_SHA = "75ea30f1f9751ca1068cc1e782d8da7900c8e470"; - public static final String GIT_DATE = "2026-02-26 17:07:05 CST"; + public static final int GIT_REVISION = 131; + public static final String GIT_SHA = "35221cd0d802b8bd923e88733fee4bee72f92344"; + public static final String GIT_DATE = "2026-02-26 17:36:03 CST"; public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-02-26 17:32:21 CST"; - public static final long BUILD_UNIX_TIME = 1772148741165L; + public static final String BUILD_DATE = "2026-02-26 19:12:26 CST"; + public static final long BUILD_UNIX_TIME = 1772154746562L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index b848a779..e09a3f7e 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -191,7 +191,7 @@ private void configureAutoBuilder() { config, // Assume the path needs to be flipped for Red vs Blue, this is normally the case () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - drivetrain // Subsystem for requirements + this // Subsystem for requirements ); } catch (Exception ex) { DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); diff --git a/src/main/java/frc/robot/subsystems/vision/limelight/LimelightSubsystem.java b/src/main/java/frc/robot/subsystems/vision/limelight/LimelightSubsystem.java index 70d3189c..09c4e08d 100644 --- a/src/main/java/frc/robot/subsystems/vision/limelight/LimelightSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/limelight/LimelightSubsystem.java @@ -130,11 +130,11 @@ public void addOdometryMeasurement() { Logger.recordOutput("Limelight/" + limelightName + "/Pose", transformedPose); // Add measurement to drivetrain pose estimator - drivetrain.addVisionMeasurement(transformedPose, estimate.timestampSeconds, LimelightConstants.VISION_STD_DEVS); + drivetrain.addVisionMeasurement(transformedPose, estimate.timestampSeconds, LimelightConstants.calculateQuestUpdateStdDevs(estimate)); // Add measurement to QuestNav pose estimator if enabled if (QuestNavConstants.USE_LIMELIGHT_FOR_VISION_MEASUREMENTS) { - questNavSubsystem.addVisionMeasurement(transformedPose, estimate.timestampSeconds, LimelightConstants.VISION_STD_DEVS, estimate); + questNavSubsystem.addVisionMeasurement(transformedPose, estimate.timestampSeconds, LimelightConstants.calculateQuestUpdateStdDevs(estimate), estimate); } } diff --git a/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java b/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java index b74a8cd6..350bc0d4 100644 --- a/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java @@ -22,9 +22,9 @@ public class QuestNavConstants { */ public static final Matrix ODOMETRY_STD_DEVS = VecBuilder.fill( - 0.05, // 5cm standard deviation in X - 0.05, // 5cm standard deviation in Y - 0.01 // ~0.57 degrees standard deviation in rotation + 0.15, + 0.15, + 0.15 ); /** From 82294b4fa930d1e9e5eae1d692905e95e0d42cbe Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Thu, 26 Feb 2026 19:57:00 -0600 Subject: [PATCH 008/102] stuff idk --- .../deploy/pathplanner/paths/HDHOH - Part 2.path | 16 +--------------- .../vision/questnav/QuestNavConstants.java | 6 +++--- 2 files changed, 4 insertions(+), 18 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path b/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path index fcfcd5e7..b058375b 100644 --- a/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path +++ b/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path @@ -61,21 +61,7 @@ } ], "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.5793345494923854, - "maxWaypointRelativePos": 2.216301157994923, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], + "constraintZones": [], "pointTowardsZones": [ { "fieldPosition": { diff --git a/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java b/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java index 350bc0d4..f17630bc 100644 --- a/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java @@ -33,9 +33,9 @@ public class QuestNavConstants { */ public static final Matrix QUESTNAV_STD_DEVS = VecBuilder.fill( - 0.02, // 2cm standard deviation in X - 0.02, // 2cm standard deviation in Y - 0.035 // 2deg/0.035rad standard deviation in rotation + 0.05, + 0.05, + 0.1 ); public static final boolean USE_LIMELIGHT_FOR_VISION_MEASUREMENTS = true; From 867e7785f9cd6803db57be684d9c96a834def330 Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Sat, 28 Feb 2026 17:26:50 -0600 Subject: [PATCH 009/102] AHHHHHHHHHHHH idk. Tried to get a motor to work --- src/main/java/frc/robot/BuildConstants.java | 10 +++---- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/joysticks/DebugJoystick.java | 26 +++++++++++++++++-- .../mechanisms/shooter/ShooterSubsystem.java | 20 +++++++++++++- 5 files changed, 50 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index ef5446aa..1c390b68 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 131; - public static final String GIT_SHA = "35221cd0d802b8bd923e88733fee4bee72f92344"; - public static final String GIT_DATE = "2026-02-26 17:36:03 CST"; + public static final int GIT_REVISION = 133; + public static final String GIT_SHA = "82294b4fa930d1e9e5eae1d692905e95e0d42cbe"; + public static final String GIT_DATE = "2026-02-26 19:57:00 CST"; public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-02-26 19:12:26 CST"; - public static final long BUILD_UNIX_TIME = 1772154746562L; + public static final String BUILD_DATE = "2026-02-28 17:23:53 CST"; + public static final long BUILD_UNIX_TIME = 1772321033341L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a5d665b6..844bb694 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -105,7 +105,7 @@ public void robotInit() { robotContainer.shooterSubsystem.shooterPitchMotor, robotContainer.climberSubsystem.climberLeft, robotContainer.climberSubsystem.climberRight - ), NeutralModeValue.Brake); + ), NeutralModeValue.Coast); Logger.recordOutput("FieldInfo/CurrentAlliance", currentAlliance.toString()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 256af0a6..d974b213 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -59,7 +59,7 @@ public class RobotContainer { // MARK: Xbox Controllers public final DriverJoystick driverJoystick = new DriverJoystick(new CommandXboxController(0), drivetrain); public final OperatorJoystick operatorJoystick = new OperatorJoystick(new CommandXboxController(1), drivetrain, intakeSubsystem); - public final DebugJoystick debugJoystick = new DebugJoystick(new CommandXboxController(2), drivetrain); + public final DebugJoystick debugJoystick = new DebugJoystick(new CommandXboxController(2), drivetrain, shooterSubsystem); diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 3d1d3707..7114b932 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -3,22 +3,34 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.networktables.DoubleEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; import frc.robot.subsystems.vision.limelight.LimelightConstants; import frc.robot.subsystems.vision.limelight.LimelightHelpers; public class DebugJoystick { public final CommandXboxController joystick; private final Drive drivetrain; + private final ShooterSubsystem shooterSubsystem; + private final DoubleEntry shooterTargetTest; public DebugJoystick( CommandXboxController joystick, - Drive drivetrain + Drive drivetrain, + ShooterSubsystem shooterSubsystem ) { this.joystick = joystick; this.drivetrain = drivetrain; + this.shooterSubsystem = shooterSubsystem; + + NetworkTable table = NetworkTableInstance.getDefault().getTable("ShooterSubsystem"); + + shooterTargetTest = table.getDoubleTopic("ShooterAngleEntry").getEntry(1.0); } public void configureBindings() { @@ -26,7 +38,17 @@ public void configureBindings() { joystick.b(); - joystick.x(); + joystick.x().onTrue( + Commands.runOnce( + () -> { + System.out.println("adsifuhasdlifuhsadfliuhasdf"); + System.out.println(shooterTargetTest.get()); + System.out.println(shooterSubsystem.shooterPitchMotor.getMotor().get()); + shooterSubsystem.pitchToAngleDeg(shooterTargetTest.get()); + // shooterSubsystem.shooterPitchMotor.set(0.3); + } + ) + ); joystick.y(); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 32bbaa83..4b5cb3ca 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -10,6 +10,9 @@ import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.networktables.DoubleEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -36,6 +39,8 @@ public class ShooterSubsystem extends SubsystemBase { TreeMap dataPoints = new TreeMap<>(); + private final DoubleEntry shooterTargetTest; + List shooterDataPoints = List.of( // TODO: Collect successful data points and add them here new ShooterDataPoint(0, 0, 0) @@ -51,6 +56,12 @@ public ShooterSubsystem(Drive drivetrain) { for (ShooterDataPoint point : shooterDataPoints) { dataPoints.put(point.distance, point); } + + NetworkTable table = NetworkTableInstance.getDefault().getTable("ShooterSubsystem"); + + shooterTargetTest = table.getDoubleTopic("ShooterAngleEntry").getEntry(1.0); + + shooterTargetTest.set(1.0); } /** Motor controlling the shooter pitch (angle). */ @@ -108,9 +119,16 @@ public ShooterSubsystem(Drive drivetrain) { shooterPitchHoldSpeed, positionThreshold, 0.05, - () -> getShooterPitchDeg() + //() -> getShooterPitchDeg() + () -> shooterPitchMotor.getMotor().get() ); + + @Override + public void periodic() { + // shooterPositionManager.positionTargetManagement(); + } + // --- Commands --- /** From 82fbb37d91afd537e50b46bd61be054a6e1bbaa9 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Sat, 28 Feb 2026 17:53:13 -0600 Subject: [PATCH 010/102] Updated WhatTime to v2026.1.16-beta.5 --- src/main/java/frc/robot/BuildConstants.java | 12 ++++++------ .../mechanisms/intake/IntakeSubsystem.java | 1 + .../mechanisms/shooter/ShooterSubsystem.java | 5 +++-- vendordeps/WhatTime.json | 4 ++-- 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 1c390b68..486c0fea 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,14 +5,14 @@ */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "roomba"; + public static final String MAVEN_NAME = "2026 Roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 133; - public static final String GIT_SHA = "82294b4fa930d1e9e5eae1d692905e95e0d42cbe"; - public static final String GIT_DATE = "2026-02-26 19:57:00 CST"; + public static final int GIT_REVISION = 134; + public static final String GIT_SHA = "867e7785f9cd6803db57be684d9c96a834def330"; + public static final String GIT_DATE = "2026-02-28 17:26:50 CST"; public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-02-28 17:23:53 CST"; - public static final long BUILD_UNIX_TIME = 1772321033341L; + public static final String BUILD_DATE = "2026-02-28 17:52:32 CST"; + public static final long BUILD_UNIX_TIME = 1772322752365L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index aa02d498..133d4eac 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -40,6 +40,7 @@ public void periodic() { 0.0, threshold, 0.02, + 0.01, () -> intakeWheelsMotor.getPosition() // Use motor encoder for position ); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 4b5cb3ca..cdea3d87 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -95,7 +95,7 @@ public ShooterSubsystem(Drive drivetrain) { public double shooterPitchHoldSpeed = 0.0; /** Maximum allowed shooter pitch (units depend on mechanism, e.g., rotations or percent). */ - public double shooterPitchMax = 0.3; + public double shooterPitchMax = 10.0; /** Minimum allowed shooter pitch. */ public double shooterPitchMin = 0.0; @@ -119,8 +119,9 @@ public ShooterSubsystem(Drive drivetrain) { shooterPitchHoldSpeed, positionThreshold, 0.05, + 0.01, //() -> getShooterPitchDeg() - () -> shooterPitchMotor.getMotor().get() + () -> shooterPitchMotor.getMotor().getPosition().getValueAsDouble() ); diff --git a/vendordeps/WhatTime.json b/vendordeps/WhatTime.json index 54fcac61..7d480c07 100644 --- a/vendordeps/WhatTime.json +++ b/vendordeps/WhatTime.json @@ -1,7 +1,7 @@ { "fileName": "WhatTime.json", "name": "WhatTime", - "version": "v2026.1.16-beta.3", + "version": "v2026.1.16-beta.5", "frcYear": 2026, "uuid": "2C0B26E9-47AA-4C55-B235-640C45ECAD33", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.btwrobotics.whattime.frc", "artifactId": "whattime", - "version": "v2026.1.16-beta.3" + "version": "v2026.1.16-beta.5" } ], "jniDependencies": [], From 488e3e16f4c80561c19e04665cbc1f28e39612f3 Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Mon, 2 Mar 2026 00:08:07 -0600 Subject: [PATCH 011/102] Updated TunerConstants.java for the MK4n swerve modules on the new robot. --- .../frc/robot/generated/TunerConstants.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index b9c7c2fc..b9adaa65 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -25,7 +25,7 @@ public class TunerConstants { // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() .withKP(100).withKI(0).withKD(0.5) - .withKS(0.1).withKV(2.66).withKA(0) + .withKS(0.1).withKV(2.33).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput @@ -74,14 +74,14 @@ public class TunerConstants { // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.58); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.23); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.5714285714285716; + private static final double kCoupleRatio = 3.125; - private static final double kDriveGearRatio = 6.746031746031747; - private static final double kSteerGearRatio = 21.428571428571427; + private static final double kDriveGearRatio = 5.902777777777778; + private static final double kSteerGearRatio = 18.75; private static final Distance kWheelRadius = Inches.of(2); private static final boolean kInvertLeftSide = false; @@ -129,7 +129,7 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 2; private static final int kFrontLeftSteerMotorId = 1; private static final int kFrontLeftEncoderId = 30; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.406005859375); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.320556640625); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; @@ -140,7 +140,7 @@ public class TunerConstants { private static final int kFrontRightDriveMotorId = 4; private static final int kFrontRightSteerMotorId = 3; private static final int kFrontRightEncoderId = 31; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.355224609375); + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.132568359375); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; @@ -151,7 +151,7 @@ public class TunerConstants { private static final int kBackLeftDriveMotorId = 8; private static final int kBackLeftSteerMotorId = 7; private static final int kBackLeftEncoderId = 33; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.009765625); + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.114501953125); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; @@ -162,7 +162,7 @@ public class TunerConstants { private static final int kBackRightDriveMotorId = 6; private static final int kBackRightSteerMotorId = 5; private static final int kBackRightEncoderId = 32; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.1044921875); + private static final Angle kBackRightEncoderOffset = Rotations.of(0.062744140625); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; From 0aa8a5c116478e9f5f08a66a7e65211612e9fa4a Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Mon, 2 Mar 2026 00:55:49 -0600 Subject: [PATCH 012/102] Updates to intake and shooter system to make them have correct limits --- src/main/java/frc/robot/BuildConstants.java | 12 ++++----- src/main/java/frc/robot/Robot.java | 5 ++-- .../mechanisms/intake/IntakeSubsystem.java | 26 +++++++++--------- .../mechanisms/shooter/ShooterSubsystem.java | 27 ++++++++++++++----- 4 files changed, 42 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 486c0fea..e78bff22 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,14 +5,14 @@ */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026 Roomba"; + public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 134; - public static final String GIT_SHA = "867e7785f9cd6803db57be684d9c96a834def330"; - public static final String GIT_DATE = "2026-02-28 17:26:50 CST"; + public static final int GIT_REVISION = 136; + public static final String GIT_SHA = "488e3e16f4c80561c19e04665cbc1f28e39612f3"; + public static final String GIT_DATE = "2026-03-02 00:08:07 CST"; public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-02-28 17:52:32 CST"; - public static final long BUILD_UNIX_TIME = 1772322752365L; + public static final String BUILD_DATE = "2026-03-02 00:54:54 CST"; + public static final long BUILD_UNIX_TIME = 1772434494035L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 844bb694..51dc8757 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -104,8 +104,9 @@ public void robotInit() { motorBulkActions.setNeutralModeBulk(Arrays.asList( robotContainer.shooterSubsystem.shooterPitchMotor, robotContainer.climberSubsystem.climberLeft, - robotContainer.climberSubsystem.climberRight - ), NeutralModeValue.Coast); + robotContainer.climberSubsystem.climberRight, + robotContainer.intakeSubsystem.angleMotor + ), NeutralModeValue.Brake); Logger.recordOutput("FieldInfo/CurrentAlliance", currentAlliance.toString()); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 133d4eac..052291a1 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -11,19 +11,17 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeSubsystem extends SubsystemBase { - private final double minValue = 0.0; - private final double maxValue = 1.0; - private final double threshold = 0.2; + private final double minValue = -8.0; + private final double maxValue = -0.5; + private final double threshold = 0.5; - private List angleMotors = List.of( - new MotorWrapper( - new TalonFX(9), false - ) + public MotorWrapper angleMotor = new MotorWrapper( + new TalonFX(9), false ); @Override public void periodic() { - Logger.recordOutput("IntakeSubsystem/Angle", angleMotors.get(0).getPosition()); + Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getPosition()); } private MotorWrapper intakeWheelsMotor = new MotorWrapper( @@ -35,13 +33,13 @@ public void periodic() { private PositionManager intakePositionManager = new PositionManager( minValue, maxValue, - angleMotors, + List.of(angleMotor), 0.2, - 0.0, + 0.05, threshold, - 0.02, - 0.01, - () -> intakeWheelsMotor.getPosition() // Use motor encoder for position + 0.1, + 0.1, + () -> angleMotor.getPosition() // Use motor encoder for position ); public void setPosition(double targetPosition) { @@ -49,7 +47,7 @@ public void setPosition(double targetPosition) { intakePositionManager.setTarget(targetPosition); } - private double intakeSpeed = 0.5; + private double intakeSpeed = 0.2; public void setIntake(IntakeStates intakeState) { Logger.recordOutput("IntakeSubsystem/State", intakeState.toString()); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index cdea3d87..998babe9 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -4,9 +4,13 @@ import java.util.Map; import java.util.TreeMap; +import org.littletonrobotics.junction.Logger; + import com.btwrobotics.WhatTime.frc.FlywheelPair; import com.btwrobotics.WhatTime.frc.MotorManagers.MotorWrapper; import com.btwrobotics.WhatTime.frc.MotorManagers.PositionManager; +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.TalonFX; @@ -84,6 +88,8 @@ public ShooterSubsystem(Drive drivetrain) { /** IMU sensor for shooter orientation feedback. */ public final Pigeon2 shooterPigeon = new Pigeon2(34); + public final CANcoder shooterThroughBore = new CANcoder(36); + // --- Shooter configuration and tuning fields --- /** Entry angle to the hub in degrees (TODO: calculate actual value). */ @@ -95,12 +101,12 @@ public ShooterSubsystem(Drive drivetrain) { public double shooterPitchHoldSpeed = 0.0; /** Maximum allowed shooter pitch (units depend on mechanism, e.g., rotations or percent). */ - public double shooterPitchMax = 10.0; + public double shooterPitchMax = 0.6; /** Minimum allowed shooter pitch. */ public double shooterPitchMin = 0.0; /** Threshold for position manager to consider the shooter "at position". */ - public double positionThreshold = 1.0; + public double positionThreshold = 0.02; /** Height of the hub (target) in meters. */ // public double hubHeight = 2; @@ -119,14 +125,16 @@ public ShooterSubsystem(Drive drivetrain) { shooterPitchHoldSpeed, positionThreshold, 0.05, - 0.01, - //() -> getShooterPitchDeg() - () -> shooterPitchMotor.getMotor().getPosition().getValueAsDouble() + 0.1, + () -> getThroughBorePosition() + // () -> getShooterPitchDeg() + // () -> shooterPitchMotor.getMotor().getPosition().getValueAsDouble() ); @Override public void periodic() { + Logger.recordOutput("ShooterSubsystem/ThroughBoreAngle", getThroughBorePosition()); // shooterPositionManager.positionTargetManagement(); } @@ -139,6 +147,12 @@ public void periodic() { public void pitchToAngleDeg(double angle) { shooterPositionManager.setTarget(angle); } + + // MARK: Get Through Bore + public double getThroughBorePosition() { + double offset = 0.4; + return shooterThroughBore.getAbsolutePosition().getValueAsDouble() + offset; + } /** * Returns a command to aim the shooter at the hub. @@ -177,7 +191,8 @@ public double getShooterMotorPitchDeg() { * @return shooter pitch in degrees (roll axis) */ public double getShooterPitchDeg() { - return shooterPigeon.getRoll().getValueAsDouble(); + // return shooterPigeon.getRoll().getValueAsDouble(); + return shooterThroughBore.getAbsolutePosition().getValueAsDouble(); } public UpperLowerPoint shooterUpperLower() { From 1835302c339e73b22e1acee97935b125390cea40 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Mon, 2 Mar 2026 16:37:34 -0600 Subject: [PATCH 013/102] I swear to god, mechanisms better finish their stuff on time or I will crash out. --- src/main/java/frc/robot/RobotContainer.java | 2 -- .../subsystems/mechanisms/climber/ClimberSubsystem.java | 8 ++------ .../subsystems/mechanisms/shooter/ShooterSubsystem.java | 2 +- 3 files changed, 3 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d974b213..a0e28a7d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,8 +4,6 @@ package frc.robot; -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; - import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; diff --git a/src/main/java/frc/robot/subsystems/mechanisms/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/climber/ClimberSubsystem.java index 7af5ad81..3b232675 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/climber/ClimberSubsystem.java @@ -18,16 +18,13 @@ public class ClimberSubsystem extends SubsystemBase { public static final double minHeight = 0.0; public static final double maxHeight = 65.0; - - - // TODO: VERY IMPORTANT!!!! Put the correct device ids public final MotorWrapper climberLeft = new MotorWrapper( - new TalonFX(12), + new TalonFX(63), true ); public final MotorWrapper climberRight = new MotorWrapper( - new TalonFX(13), + new TalonFX(64), false ); public final FlywheelPair climberPair = new FlywheelPair(climberLeft, climberRight, climberUpDownSpeed); @@ -45,7 +42,6 @@ public double getClimberHeight() { } - // TODO: Caedmon mentioned something about this being implemented differently, needs to be done (like we dont have to make the motors rotate opposite) public Command climberUp() { return Commands.run( () -> { diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 998babe9..6a2744f1 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -172,7 +172,7 @@ public Command aimAtHub() { pitchToAngleDeg(hubCalculateDataPoint.angle); shooterMotors.setSpeed(hubCalculateDataPoint.speed); - } + }, this ); } From e54f1172bd8c9d3ad6355a371a1ffa5f1554b11a Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Tue, 3 Mar 2026 18:05:51 -0600 Subject: [PATCH 014/102] Updated position values. Updated WhatTime. Updated joysticks. Did a few other things. --- src/main/java/frc/robot/BuildConstants.java | 10 ++--- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/joysticks/DriverJoystick.java | 40 +++++++++++++++++-- .../mechanisms/shooter/ShooterSubsystem.java | 13 ++++++ .../vision/limelight/LimelightConstants.java | 2 +- .../vision/questnav/QuestNavConstants.java | 4 +- vendordeps/WhatTime.json | 4 +- 7 files changed, 60 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index e78bff22..8c8243c2 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 136; - public static final String GIT_SHA = "488e3e16f4c80561c19e04665cbc1f28e39612f3"; - public static final String GIT_DATE = "2026-03-02 00:08:07 CST"; + public static final int GIT_REVISION = 138; + public static final String GIT_SHA = "1835302c339e73b22e1acee97935b125390cea40"; + public static final String GIT_DATE = "2026-03-02 16:37:34 CST"; public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-03-02 00:54:54 CST"; - public static final long BUILD_UNIX_TIME = 1772434494035L; + public static final String BUILD_DATE = "2026-03-03 17:39:43 CST"; + public static final long BUILD_UNIX_TIME = 1772581183821L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a0e28a7d..60c8db65 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -55,7 +55,7 @@ public class RobotContainer { public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(drivetrain); // MARK: Xbox Controllers - public final DriverJoystick driverJoystick = new DriverJoystick(new CommandXboxController(0), drivetrain); + public final DriverJoystick driverJoystick = new DriverJoystick(new CommandXboxController(0), drivetrain, shooterSubsystem, intakeSubsystem); public final OperatorJoystick operatorJoystick = new OperatorJoystick(new CommandXboxController(1), drivetrain, intakeSubsystem); public final DebugJoystick debugJoystick = new DebugJoystick(new CommandXboxController(2), drivetrain, shooterSubsystem); diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index 9bdd309b..09c6ce08 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -6,19 +6,27 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; +import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; import frc.robot.subsystems.vision.limelight.LimelightConstants; import frc.robot.subsystems.vision.limelight.LimelightHelpers; public class DriverJoystick { public final CommandXboxController joystick; private final Drive drivetrain; + private final ShooterSubsystem shooterSubsystem; + private final IntakeSubsystem intakeSubsystem; public DriverJoystick( CommandXboxController joystick, - Drive drivetrain + Drive drivetrain, + ShooterSubsystem shooterSubsystem, + IntakeSubsystem intakeSubsystem ) { this.joystick = joystick; this.drivetrain = drivetrain; + this.shooterSubsystem = shooterSubsystem; + this.intakeSubsystem = intakeSubsystem; } public void configureBindings() { @@ -38,11 +46,35 @@ public void configureBindings() { joystick.y(); - joystick.rightTrigger(); + joystick.rightTrigger().whileTrue( + Commands.run( + () -> { + shooterSubsystem.feedMotor.set(0.5); + } + ) + ).onFalse( + Commands.run( + () -> { + shooterSubsystem.feedMotor.set(0.0); + } + ) + ); - joystick.leftTrigger(); + joystick.leftTrigger().onTrue( + Commands.run( + () -> { + intakeSubsystem.setIntake(null); + } + ) + ); - joystick.rightBumper(); + joystick.rightBumper().onTrue( + Commands.runOnce( + () -> { + shooterSubsystem.toggleShooterMotors(); + } + ) + ); joystick.leftBumper(); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 6a2744f1..a4b95fd4 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -131,11 +131,24 @@ public ShooterSubsystem(Drive drivetrain) { // () -> shooterPitchMotor.getMotor().getPosition().getValueAsDouble() ); + public boolean shooterFlywheelsEnabled = false; + @Override public void periodic() { Logger.recordOutput("ShooterSubsystem/ThroughBoreAngle", getThroughBorePosition()); // shooterPositionManager.positionTargetManagement(); + if (shooterFlywheelsEnabled) { + shooterMotors.setSpeed(0.5); + } + else { + shooterMotors.setSpeed(0.0); + } + } + + + public void toggleShooterMotors() { + shooterFlywheelsEnabled = !shooterFlywheelsEnabled; } // --- Commands --- diff --git a/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java b/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java index 80ab19e8..6e1c595c 100644 --- a/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java @@ -23,7 +23,7 @@ public class LimelightConstants { // Position of Limelight relative to the centre of the robot in metres public static final Transform2d LIMELIGHT_4_TRANSFORM_FROM_CENTRE = new Transform2d( - new Translation2d(0.3429, 0.0), + new Translation2d(0.0762, 0.228), new Rotation2d(0) ); diff --git a/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java b/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java index f17630bc..1a9f809e 100644 --- a/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java @@ -12,8 +12,8 @@ public class QuestNavConstants { // 🏳️‍⚧️ TRANS-form 3d public static final Transform3d ROBOT_TO_QUEST = new Transform3d( - new Translation3d(0.0, -0.4, 0.45), - new Rotation3d(new Rotation2d(-0.5 * Math.PI)) + new Translation3d(0.0762, 0.254, 0.3556), + new Rotation3d(0.5 * Math.PI, 0.0, 0.0) ); /** diff --git a/vendordeps/WhatTime.json b/vendordeps/WhatTime.json index 7d480c07..08f1b443 100644 --- a/vendordeps/WhatTime.json +++ b/vendordeps/WhatTime.json @@ -1,7 +1,7 @@ { "fileName": "WhatTime.json", "name": "WhatTime", - "version": "v2026.1.16-beta.5", + "version": "v2026.1.16-beta.6", "frcYear": 2026, "uuid": "2C0B26E9-47AA-4C55-B235-640C45ECAD33", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.btwrobotics.whattime.frc", "artifactId": "whattime", - "version": "v2026.1.16-beta.5" + "version": "v2026.1.16-beta.6" } ], "jniDependencies": [], From 861b2b7070a3d61515462e4071ef7affe8daeb5a Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Tue, 3 Mar 2026 18:39:14 -0600 Subject: [PATCH 015/102] Did stuff. I don't care anymore. --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/joysticks/DriverJoystick.java | 11 +++++++++-- .../subsystems/mechanisms/intake/IntakeSubsystem.java | 2 -- 3 files changed, 14 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 8c8243c2..820b81a2 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 138; - public static final String GIT_SHA = "1835302c339e73b22e1acee97935b125390cea40"; - public static final String GIT_DATE = "2026-03-02 16:37:34 CST"; + public static final int GIT_REVISION = 139; + public static final String GIT_SHA = "e54f1172bd8c9d3ad6355a371a1ffa5f1554b11a"; + public static final String GIT_DATE = "2026-03-03 18:05:51 CST"; public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-03-03 17:39:43 CST"; - public static final long BUILD_UNIX_TIME = 1772581183821L; + public static final String BUILD_DATE = "2026-03-03 18:19:55 CST"; + public static final long BUILD_UNIX_TIME = 1772583595815L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index 09c6ce08..df07469c 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; import frc.robot.subsystems.vision.limelight.LimelightConstants; @@ -60,10 +61,16 @@ public void configureBindings() { ) ); - joystick.leftTrigger().onTrue( + joystick.leftTrigger().whileTrue( Commands.run( () -> { - intakeSubsystem.setIntake(null); + intakeSubsystem.setIntake(IntakeStates.INTAKE_IN); + } + ) + ).onFalse( + Commands.runOnce( + () -> { + intakeSubsystem.setIntake(IntakeStates.OFF); } ) ); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 052291a1..d0089d9a 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -28,8 +28,6 @@ public void periodic() { new TalonFX(10), false ); - // private CANcoder angleEncoder = new CANcoder(35); - private PositionManager intakePositionManager = new PositionManager( minValue, maxValue, From de13bf367611e6e795a874fc4c67cc01a7eed8e5 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 3 Mar 2026 19:09:21 -0600 Subject: [PATCH 016/102] did things. I'm going crazy --- src/main/java/frc/robot/joysticks/DriverJoystick.java | 2 +- .../subsystems/mechanisms/shooter/ShooterSubsystem.java | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index df07469c..b296e93f 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -50,7 +50,7 @@ public void configureBindings() { joystick.rightTrigger().whileTrue( Commands.run( () -> { - shooterSubsystem.feedMotor.set(0.5); + shooterSubsystem.feedMotor.set(0.2); } ) ).onFalse( diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index a4b95fd4..df2d9a8c 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -43,7 +43,7 @@ public class ShooterSubsystem extends SubsystemBase { TreeMap dataPoints = new TreeMap<>(); - private final DoubleEntry shooterTargetTest; + private final DoubleEntry shooterSpeedTest; List shooterDataPoints = List.of( // TODO: Collect successful data points and add them here @@ -63,9 +63,9 @@ public ShooterSubsystem(Drive drivetrain) { NetworkTable table = NetworkTableInstance.getDefault().getTable("ShooterSubsystem"); - shooterTargetTest = table.getDoubleTopic("ShooterAngleEntry").getEntry(1.0); + shooterSpeedTest = table.getDoubleTopic("ShooterSpeed").getEntry(0.5); - shooterTargetTest.set(1.0); + shooterSpeedTest.set(0.5); } /** Motor controlling the shooter pitch (angle). */ @@ -139,7 +139,7 @@ public void periodic() { Logger.recordOutput("ShooterSubsystem/ThroughBoreAngle", getThroughBorePosition()); // shooterPositionManager.positionTargetManagement(); if (shooterFlywheelsEnabled) { - shooterMotors.setSpeed(0.5); + shooterMotors.setSpeed(shooterSpeedTest.get()); } else { shooterMotors.setSpeed(0.0); From 7d669caec72ab9549bd5a28ac2b005ec76fb7ec2 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 3 Mar 2026 22:53:51 -0600 Subject: [PATCH 017/102] Updated paths. Updated WhatTime. Made joystick changes. Added zeroing on initialization. Removed some position manager stuff. --- .../pathplanner/paths/HDH - Part 1.path | 20 ++++----- .../pathplanner/paths/HDH - Part 2.path | 41 +++++-------------- .../pathplanner/paths/HDH - Part 3.path | 31 +++++--------- src/main/java/frc/robot/BuildConstants.java | 12 +++--- src/main/java/frc/robot/Robot.java | 6 ++- src/main/java/frc/robot/RobotContainer.java | 18 +++----- .../namedcommands/RegisterCommands.java | 14 +++---- .../frc/robot/joysticks/DriverJoystick.java | 10 ++++- .../mechanisms/intake/IntakeSubsystem.java | 10 ++--- .../mechanisms/shooter/ShooterSubsystem.java | 28 ++++++------- .../vision/questnav/QuestNavConstants.java | 1 - vendordeps/WhatTime.json | 4 +- 12 files changed, 82 insertions(+), 113 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 1.path b/src/main/deploy/pathplanner/paths/HDH - Part 1.path index 309e9cf1..279ac836 100644 --- a/src/main/deploy/pathplanner/paths/HDH - Part 1.path +++ b/src/main/deploy/pathplanner/paths/HDH - Part 1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.666775798905367, - "y": 6.3921116922669485 + "x": 3.6576451822916667, + "y": 4.265358579282408 }, "prevControl": null, "nextControl": { - "x": 3.1429084260122266, - "y": 6.649324898279437 + "x": 3.1337778093985262, + "y": 4.522571785294896 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.7694349179025424, - "y": 6.753512259445622 + "x": 0.4183219762731487, + "y": 4.7485145399305555 }, "prevControl": { - "x": 2.6898677817295176, - "y": 6.031772232809059 + "x": 1.957807508680556, + "y": 5.192435402199075 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -43.89633519981335 + "rotation": -90.0 }, "reversed": false, "folder": "2 - Hub, Depot, Hub (HDH)", "idealStartingState": { "velocity": 0, - "rotation": -90.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 2.path b/src/main/deploy/pathplanner/paths/HDH - Part 2.path index f8857c0c..22a843da 100644 --- a/src/main/deploy/pathplanner/paths/HDH - Part 2.path +++ b/src/main/deploy/pathplanner/paths/HDH - Part 2.path @@ -3,53 +3,32 @@ "waypoints": [ { "anchor": { - "x": 1.7694349179025424, - "y": 6.753512259445622 + "x": 0.4183219762731487, + "y": 4.7485145399305555 }, "prevControl": null, "nextControl": { - "x": 1.6072378556769904, - "y": 7.415759675142259 + "x": 0.42060589674932697, + "y": 5.224863656477267 }, "isLocked": false, "linkedName": "HDH - 1" }, { "anchor": { - "x": 0.40981588762358767, - "y": 6.858705033986582 + "x": 0.4183219762731487, + "y": 6.927243489583334 }, "prevControl": { - "x": 0.4479510058488559, - "y": 7.105779340133427 - }, - "nextControl": { - "x": 0.34899375441384184, - "y": 6.464643416754943 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.40981588762358767, - "y": 5.010476694915254 - }, - "prevControl": { - "x": 0.40981588762358767, - "y": 5.3825688815658905 + "x": 0.42114279554876993, + "y": 7.177227575034016 }, "nextControl": null, "isLocked": false, "linkedName": "HDH - 2" } ], - "rotationTargets": [ - { - "waypointRelativePos": 1.0, - "rotationDegrees": -90.0 - } - ], + "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -69,7 +48,7 @@ "folder": "2 - Hub, Depot, Hub (HDH)", "idealStartingState": { "velocity": 0, - "rotation": -43.89633519981335 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 3.path b/src/main/deploy/pathplanner/paths/HDH - Part 3.path index 7d0bfdbb..92346bf1 100644 --- a/src/main/deploy/pathplanner/paths/HDH - Part 3.path +++ b/src/main/deploy/pathplanner/paths/HDH - Part 3.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 0.40981588762358767, - "y": 5.010476694915254 + "x": 0.4183219762731487, + "y": 6.927243489583334 }, "prevControl": null, "nextControl": { - "x": 1.3816289629778247, - "y": 4.955293945190116 + "x": 2.097426721643519, + "y": 6.502415292245371 }, "isLocked": false, "linkedName": "HDH - 2" }, { "anchor": { - "x": 2.276431188206215, - "y": 6.445772587835452 + "x": 3.6354688223379634, + "y": 4.255910662615741 }, "prevControl": { - "x": 1.3293245911501488, - "y": 5.973136390129629 + "x": 2.313219762731482, + "y": 4.215888237847222 }, "nextControl": null, "isLocked": false, @@ -30,18 +30,7 @@ ], "rotationTargets": [], "constraintZones": [], - "pointTowardsZones": [ - { - "fieldPosition": { - "x": 4.626, - "y": 4.034 - }, - "rotationOffset": 0.0, - "minWaypointRelativePos": 0.36584906408629436, - "maxWaypointRelativePos": 1.0, - "name": "Point Towards Zone" - } - ], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { "maxVelocity": 1.5, @@ -53,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -48.676167316772535 + "rotation": 0.0 }, "reversed": false, "folder": "2 - Hub, Depot, Hub (HDH)", diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 820b81a2..bb5d439c 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,14 +5,14 @@ */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "roomba"; + public static final String MAVEN_NAME = "2026 Roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 139; - public static final String GIT_SHA = "e54f1172bd8c9d3ad6355a371a1ffa5f1554b11a"; - public static final String GIT_DATE = "2026-03-03 18:05:51 CST"; + public static final int GIT_REVISION = 141; + public static final String GIT_SHA = "de13bf367611e6e795a874fc4c67cc01a7eed8e5"; + public static final String GIT_DATE = "2026-03-03 19:09:21 CST"; public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-03-03 18:19:55 CST"; - public static final long BUILD_UNIX_TIME = 1772583595815L; + public static final String BUILD_DATE = "2026-03-03 22:30:57 CST"; + public static final long BUILD_UNIX_TIME = 1772598657081L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 51dc8757..f0bed600 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -103,11 +103,13 @@ public void robotInit() { motorBulkActions.setNeutralModeBulk(Arrays.asList( robotContainer.shooterSubsystem.shooterPitchMotor, - robotContainer.climberSubsystem.climberLeft, - robotContainer.climberSubsystem.climberRight, robotContainer.intakeSubsystem.angleMotor ), NeutralModeValue.Brake); + // Reset motor speeds to zero. + robotContainer.intakeSubsystem.angleMotor.set(0.0); + robotContainer.shooterSubsystem.shooterPitchMotor.set(0.0); + Logger.recordOutput("FieldInfo/CurrentAlliance", currentAlliance.toString()); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 60c8db65..4471250a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,10 +24,9 @@ import frc.robot.joysticks.OperatorJoystick; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; -import frc.robot.subsystems.mechanisms.climber.ClimberSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; -import frc.robot.subsystems.motor.MotorSubsystem; + public class RobotContainer { // public static double MAX_SPEED = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed @@ -49,27 +48,22 @@ public class RobotContainer { // LimelightSubsystem limelight2Subsystem = new LimelightSubsystem(drivetrain, "limelight-two"); // MARK: Subsystems - public final ClimberSubsystem climberSubsystem = new ClimberSubsystem(); + // public final ClimberSubsystem climberSubsystem = new ClimberSubsystem(); public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); - public final MotorSubsystem motorSubsystem = new MotorSubsystem(); + // public final MotorSubsystem motorSubsystem = new MotorSubsystem(); public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(drivetrain); // MARK: Xbox Controllers public final DriverJoystick driverJoystick = new DriverJoystick(new CommandXboxController(0), drivetrain, shooterSubsystem, intakeSubsystem); public final OperatorJoystick operatorJoystick = new OperatorJoystick(new CommandXboxController(1), drivetrain, intakeSubsystem); public final DebugJoystick debugJoystick = new DebugJoystick(new CommandXboxController(2), drivetrain, shooterSubsystem); - - - - // MARK: Register Commands - public final RegisterCommands registerCommands = new RegisterCommands(intakeSubsystem, shooterSubsystem, climberSubsystem, motorSubsystem); + public final RegisterCommands registerCommands = new RegisterCommands(intakeSubsystem, shooterSubsystem); // MARK: Tests - public final Tests tests = new Tests(intakeSubsystem, shooterSubsystem, climberSubsystem, motorSubsystem); - + // public final Tests tests = new Tests(intakeSubsystem, shooterSubsystem, climberSubsystem, motorSubsystem); // private final SendableChooser autoChooser; private final SendableChooser autoChooser; @@ -85,7 +79,7 @@ public RobotContainer() { // MARK: Run Tests /* Disable tests on actual code */ - tests.runTests(); + // tests.runTests(); configureBindings(); diff --git a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java index 53bef77b..60dec356 100644 --- a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java +++ b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java @@ -10,19 +10,19 @@ public class RegisterCommands { IntakeSubsystem intakeSubsystem; ShooterSubsystem shooterSubsystem; - ClimberSubsystem climberSubsystem; - MotorSubsystem motorSubsystem; + // ClimberSubsystem climberSubsystem; + // MotorSubsystem motorSubsystem; public RegisterCommands( IntakeSubsystem intakeSubsystem, - ShooterSubsystem shooterSubsystem, - ClimberSubsystem climberSubsystem, - MotorSubsystem motorSubsystem + ShooterSubsystem shooterSubsystem + // ClimberSubsystem climberSubsystem, + // MotorSubsystem motorSubsystem ) { this.intakeSubsystem = intakeSubsystem; this.shooterSubsystem = shooterSubsystem; - this.climberSubsystem = climberSubsystem; - this.motorSubsystem = motorSubsystem; + // this.climberSubsystem = climberSubsystem; + // this.motorSubsystem = motorSubsystem; } public void registerCommands(){ diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index b296e93f..03d2d243 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -54,7 +54,7 @@ public void configureBindings() { } ) ).onFalse( - Commands.run( + Commands.runOnce( () -> { shooterSubsystem.feedMotor.set(0.0); } @@ -83,7 +83,13 @@ public void configureBindings() { ) ); - joystick.leftBumper(); + joystick.leftBumper().onTrue( + Commands.runOnce( + () -> { + intakeSubsystem.setPosition(-6); + } + ) + ); joystick.povUp(); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index d0089d9a..5ccea3fc 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -12,7 +12,7 @@ public class IntakeSubsystem extends SubsystemBase { private final double minValue = -8.0; - private final double maxValue = -0.5; + private final double maxValue = 0.0; private final double threshold = 0.5; public MotorWrapper angleMotor = new MotorWrapper( @@ -32,11 +32,11 @@ public void periodic() { minValue, maxValue, List.of(angleMotor), - 0.2, - 0.05, + 0.2, // Motor Speed + 0.0, // Hold Speed threshold, - 0.1, - 0.1, + 0.1, // Min Speed + 0.1, // Kim Possible (kP) () -> angleMotor.getPosition() // Use motor encoder for position ); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index df2d9a8c..58bbee74 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -117,19 +117,19 @@ public ShooterSubsystem(Drive drivetrain) { * PositionManager for controlling the shooter pitch motor to a target angle. * Uses feedback from the shooter IMU. */ - public PositionManager shooterPositionManager = new PositionManager( - shooterPitchMin, - shooterPitchMax, - List.of(shooterPitchMotor), - 0.2, - shooterPitchHoldSpeed, - positionThreshold, - 0.05, - 0.1, - () -> getThroughBorePosition() - // () -> getShooterPitchDeg() - // () -> shooterPitchMotor.getMotor().getPosition().getValueAsDouble() - ); + // public PositionManager shooterPositionManager = new PositionManager( + // shooterPitchMin, + // shooterPitchMax, + // List.of(shooterPitchMotor), + // 0.2, + // shooterPitchHoldSpeed, + // positionThreshold, + // 0.05, + // 0.1, + // () -> getThroughBorePosition() + // // () -> getShooterPitchDeg() + // // () -> shooterPitchMotor.getMotor().getPosition().getValueAsDouble() + // ); public boolean shooterFlywheelsEnabled = false; @@ -158,7 +158,7 @@ public void toggleShooterMotors() { * @param angle target pitch angle in degrees */ public void pitchToAngleDeg(double angle) { - shooterPositionManager.setTarget(angle); + // shooterPositionManager.setTarget(angle); } // MARK: Get Through Bore diff --git a/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java b/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java index 1a9f809e..a69f2cbb 100644 --- a/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/questnav/QuestNavConstants.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; diff --git a/vendordeps/WhatTime.json b/vendordeps/WhatTime.json index 08f1b443..3fda3aec 100644 --- a/vendordeps/WhatTime.json +++ b/vendordeps/WhatTime.json @@ -1,7 +1,7 @@ { "fileName": "WhatTime.json", "name": "WhatTime", - "version": "v2026.1.16-beta.6", + "version": "v2026.1.16-beta.9", "frcYear": 2026, "uuid": "2C0B26E9-47AA-4C55-B235-640C45ECAD33", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.btwrobotics.whattime.frc", "artifactId": "whattime", - "version": "v2026.1.16-beta.6" + "version": "v2026.1.16-beta.9" } ], "jniDependencies": [], From 870fddc8f17673c2d4c12b9b26e0215d15368d4b Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 3 Mar 2026 22:55:51 -0600 Subject: [PATCH 018/102] Flywheel fixes --- .../subsystems/mechanisms/shooter/ShooterSubsystem.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 58bbee74..57381bb7 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -139,10 +139,12 @@ public void periodic() { Logger.recordOutput("ShooterSubsystem/ThroughBoreAngle", getThroughBorePosition()); // shooterPositionManager.positionTargetManagement(); if (shooterFlywheelsEnabled) { - shooterMotors.setSpeed(shooterSpeedTest.get()); + shooterMotors.setSpeed(shooterSpeedTest.get()); // Sets speed + shooterMotors.runForward(); // Runs motors } else { - shooterMotors.setSpeed(0.0); + shooterMotors.setSpeed(0.0); // Sets speed + shooterMotors.runForward(); // Runs motors } } From b4bef310093c0070d59a9c795cb01214ede2a436 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Wed, 4 Mar 2026 20:58:49 -0600 Subject: [PATCH 019/102] Good enough? --- .../frc/robot/joysticks/DebugJoystick.java | 27 +++++++++++-------- .../mechanisms/shooter/ShooterSubsystem.java | 9 ++++--- 2 files changed, 21 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 7114b932..eabcd663 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -31,6 +31,15 @@ public DebugJoystick( NetworkTable table = NetworkTableInstance.getDefault().getTable("ShooterSubsystem"); shooterTargetTest = table.getDoubleTopic("ShooterAngleEntry").getEntry(1.0); + + shooterSubsystem.setDefaultCommand( + Commands.run( + () -> { + shooterSubsystem.leftShooterMotor.set(Math.max(joystick.getLeftY(), 0.05)); + shooterSubsystem.rightShooterMotor.set(Math.max(joystick.getLeftY(), 0.05)); + } + ) + ); } public void configureBindings() { @@ -38,22 +47,18 @@ public void configureBindings() { joystick.b(); - joystick.x().onTrue( - Commands.runOnce( + joystick.x(); + + joystick.y(); + + joystick.rightTrigger().whileTrue( + Commands.run( () -> { - System.out.println("adsifuhasdlifuhsadfliuhasdf"); - System.out.println(shooterTargetTest.get()); - System.out.println(shooterSubsystem.shooterPitchMotor.getMotor().get()); - shooterSubsystem.pitchToAngleDeg(shooterTargetTest.get()); - // shooterSubsystem.shooterPitchMotor.set(0.3); + shooterSubsystem.feedMotor.set(Math.min(joystick.getLeftTriggerAxis(), 0.5)); } ) ); - joystick.y(); - - joystick.rightTrigger(); - joystick.leftTrigger(); joystick.rightBumper(); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 57381bb7..cb663520 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -8,8 +8,6 @@ import com.btwrobotics.WhatTime.frc.FlywheelPair; import com.btwrobotics.WhatTime.frc.MotorManagers.MotorWrapper; -import com.btwrobotics.WhatTime.frc.MotorManagers.PositionManager; -import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.TalonFX; @@ -74,9 +72,12 @@ public ShooterSubsystem(Drive drivetrain) { false ); + public MotorWrapper leftShooterMotor = new MotorWrapper(new TalonFX(13), false); + public MotorWrapper rightShooterMotor = new MotorWrapper(new TalonFX(14), true); + public FlywheelPair shooterMotors = new FlywheelPair( - new MotorWrapper(new TalonFX(13), false), // Left shooter motor - new MotorWrapper(new TalonFX(14), true), // Right shooter motor + leftShooterMotor, + rightShooterMotor, 0.4 ); From e247bade5846573490e4869dad742916017c908f Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Wed, 4 Mar 2026 21:19:22 -0600 Subject: [PATCH 020/102] Added constants and stuff --- .../mechanisms/intake/IntakeConstants.java | 7 ++++ .../mechanisms/intake/IntakeSubsystem.java | 10 ++---- .../mechanisms/shooter/ShooterConstants.java | 33 +++++++++++++++++++ .../mechanisms/shooter/ShooterSubsystem.java | 32 +----------------- 4 files changed, 44 insertions(+), 38 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java create mode 100644 src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java new file mode 100644 index 00000000..fdd680e5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.mechanisms.intake; + +public class IntakeConstants { + public static final double minValue = 0.0; + public static final double maxValue = 7.0; + public static final double threshold = 0.5; +} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 5ccea3fc..98b23f70 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -11,10 +11,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeSubsystem extends SubsystemBase { - private final double minValue = -8.0; - private final double maxValue = 0.0; - private final double threshold = 0.5; - public MotorWrapper angleMotor = new MotorWrapper( new TalonFX(9), false ); @@ -29,12 +25,12 @@ public void periodic() { ); private PositionManager intakePositionManager = new PositionManager( - minValue, - maxValue, + IntakeConstants.minValue, + IntakeConstants.maxValue, List.of(angleMotor), 0.2, // Motor Speed 0.0, // Hold Speed - threshold, + IntakeConstants.threshold, 0.1, // Min Speed 0.1, // Kim Possible (kP) () -> angleMotor.getPosition() // Use motor encoder for position diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java new file mode 100644 index 00000000..82114ac1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java @@ -0,0 +1,33 @@ +package frc.robot.subsystems.mechanisms.shooter; + +import java.util.List; + +public class ShooterConstants { + public static List shooterDataPoints = List.of( + // TODO: Collect successful data points and add them here + new ShooterDataPoint(0, 0, 0) + ); + + // --- Shooter configuration and tuning fields --- + + /** Entry angle to the hub in degrees (TODO: calculate actual value). */ + // public double hubEnterAngle = -70; + + /** Speed for pitching the shooter (open-loop, 0..1). */ + // public double shooterPitchSpeed = 0.1; + /** Hold speed for maintaining shooter pitch (open-loop, 0..1). */ + public double shooterPitchHoldSpeed = 0.0; + + /** Maximum allowed shooter pitch (units depend on mechanism, e.g., rotations or percent). */ + public double shooterPitchMax = 0.6; + /** Minimum allowed shooter pitch. */ + public double shooterPitchMin = 0.0; + + /** Threshold for position manager to consider the shooter "at position". */ + public double positionThreshold = 0.02; + + /** Height of the hub (target) in meters. */ + // public double hubHeight = 2; + /** Height of the shooter in meters. */ + // public double shooterHeight = 1; +} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index cb663520..70d0fe44 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.mechanisms.shooter; -import java.util.List; import java.util.Map; import java.util.TreeMap; @@ -43,11 +42,6 @@ public class ShooterSubsystem extends SubsystemBase { private final DoubleEntry shooterSpeedTest; - List shooterDataPoints = List.of( - // TODO: Collect successful data points and add them here - new ShooterDataPoint(0, 0, 0) - ); - /** * Constructs the ShooterSubsystem. * @param drivetrain the swerve drivetrain subsystem (for aiming/coordination) @@ -55,7 +49,7 @@ public class ShooterSubsystem extends SubsystemBase { public ShooterSubsystem(Drive drivetrain) { this.drivetrain = drivetrain; - for (ShooterDataPoint point : shooterDataPoints) { + for (ShooterDataPoint point : ShooterConstants.shooterDataPoints) { dataPoints.put(point.distance, point); } @@ -91,29 +85,6 @@ public ShooterSubsystem(Drive drivetrain) { public final CANcoder shooterThroughBore = new CANcoder(36); - // --- Shooter configuration and tuning fields --- - - /** Entry angle to the hub in degrees (TODO: calculate actual value). */ - // public double hubEnterAngle = -70; - - /** Speed for pitching the shooter (open-loop, 0..1). */ - // public double shooterPitchSpeed = 0.1; - /** Hold speed for maintaining shooter pitch (open-loop, 0..1). */ - public double shooterPitchHoldSpeed = 0.0; - - /** Maximum allowed shooter pitch (units depend on mechanism, e.g., rotations or percent). */ - public double shooterPitchMax = 0.6; - /** Minimum allowed shooter pitch. */ - public double shooterPitchMin = 0.0; - - /** Threshold for position manager to consider the shooter "at position". */ - public double positionThreshold = 0.02; - - /** Height of the hub (target) in meters. */ - // public double hubHeight = 2; - /** Height of the shooter in meters. */ - // public double shooterHeight = 1; - /** * PositionManager for controlling the shooter pitch motor to a target angle. * Uses feedback from the shooter IMU. @@ -149,7 +120,6 @@ public void periodic() { } } - public void toggleShooterMotors() { shooterFlywheelsEnabled = !shooterFlywheelsEnabled; } From aed67b9aca058b490c7e477211260716707c116a Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Wed, 4 Mar 2026 21:35:15 -0600 Subject: [PATCH 021/102] trigger and joystick controls --- src/main/java/frc/robot/BuildConstants.java | 12 ++++++------ src/main/java/frc/robot/joysticks/DebugJoystick.java | 8 ++++---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index bb5d439c..53c8f4b6 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,13 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026 Roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 141; - public static final String GIT_SHA = "de13bf367611e6e795a874fc4c67cc01a7eed8e5"; - public static final String GIT_DATE = "2026-03-03 19:09:21 CST"; + public static final int GIT_REVISION = 145; + public static final String GIT_SHA = "e247bade5846573490e4869dad742916017c908f"; + public static final String GIT_DATE = "2026-03-04 21:19:22 CST"; public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-03-03 22:30:57 CST"; - public static final long BUILD_UNIX_TIME = 1772598657081L; - public static final int DIRTY = 1; + public static final String BUILD_DATE = "2026-03-04 21:24:03 CST"; + public static final long BUILD_UNIX_TIME = 1772681043234L; + public static final int DIRTY = 0; private BuildConstants(){} } diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index eabcd663..caec2286 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -35,9 +35,9 @@ public DebugJoystick( shooterSubsystem.setDefaultCommand( Commands.run( () -> { - shooterSubsystem.leftShooterMotor.set(Math.max(joystick.getLeftY(), 0.05)); - shooterSubsystem.rightShooterMotor.set(Math.max(joystick.getLeftY(), 0.05)); - } + shooterSubsystem.leftShooterMotor.set(joystick.getLeftY()); + shooterSubsystem.rightShooterMotor.set(joystick.getLeftY()); + }, shooterSubsystem ) ); } @@ -54,7 +54,7 @@ public void configureBindings() { joystick.rightTrigger().whileTrue( Commands.run( () -> { - shooterSubsystem.feedMotor.set(Math.min(joystick.getLeftTriggerAxis(), 0.5)); + shooterSubsystem.feedMotor.set(Math.min(joystick.getRightTriggerAxis(), 0.5)); } ) ); From f579082753a45b52b0a87e1582e48a21f433bdb3 Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Thu, 5 Mar 2026 11:25:00 -0600 Subject: [PATCH 022/102] Updated paths a bit. Updated joystick controls to include control for everything. Removed NamedCommands.java. Made intake motors public. --- .../pathplanner/autos/Testing Pose Auto.auto | 6 ++++ .../pathplanner/paths/Testing Pose Path.path | 20 ++++++------- src/main/java/frc/robot/BuildConstants.java | 14 ++++----- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/commands/NamedCommands.java | 5 ---- .../namedcommands/RegisterCommands.java | 9 ++++++ .../frc/robot/joysticks/DebugJoystick.java | 29 ++++++++++++------- .../mechanisms/intake/IntakeSubsystem.java | 2 +- 8 files changed, 53 insertions(+), 34 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/NamedCommands.java diff --git a/src/main/deploy/pathplanner/autos/Testing Pose Auto.auto b/src/main/deploy/pathplanner/autos/Testing Pose Auto.auto index bbd3050b..1483d491 100644 --- a/src/main/deploy/pathplanner/autos/Testing Pose Auto.auto +++ b/src/main/deploy/pathplanner/autos/Testing Pose Auto.auto @@ -9,6 +9,12 @@ "data": { "pathName": "Testing Pose Path" } + }, + { + "type": "named", + "data": { + "name": "ShootBallsClose" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/Testing Pose Path.path b/src/main/deploy/pathplanner/paths/Testing Pose Path.path index 14accaf4..de641bcb 100644 --- a/src/main/deploy/pathplanner/paths/Testing Pose Path.path +++ b/src/main/deploy/pathplanner/paths/Testing Pose Path.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.5680509348110263, - "y": 3.836822198234916 + "x": 3.589750889679717, + "y": 4.277081850533808 }, "prevControl": null, "nextControl": { - "x": 2.8144249335901694, - "y": 3.836822198234916 + "x": 3.339750889679717, + "y": 4.277081850533808 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.799621182152162, - "y": 3.836822198234916 + "x": 3.417603795966786, + "y": 4.277081850533808 }, "prevControl": { - "x": 3.799621182152162, - "y": 3.836822198234916 + "x": 3.667603795966786, + "y": 4.277081850533808 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 53c8f4b6..2fa84566 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,15 +5,15 @@ */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026 Roomba"; + public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 145; - public static final String GIT_SHA = "e247bade5846573490e4869dad742916017c908f"; - public static final String GIT_DATE = "2026-03-04 21:19:22 CST"; + public static final int GIT_REVISION = 146; + public static final String GIT_SHA = "aed67b9aca058b490c7e477211260716707c116a"; + public static final String GIT_DATE = "2026-03-04 21:35:15 CST"; public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-03-04 21:24:03 CST"; - public static final long BUILD_UNIX_TIME = 1772681043234L; - public static final int DIRTY = 0; + public static final String BUILD_DATE = "2026-03-05 11:23:54 CST"; + public static final long BUILD_UNIX_TIME = 1772731434178L; + public static final int DIRTY = 1; private BuildConstants(){} } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4471250a..16f05eea 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -56,7 +56,7 @@ public class RobotContainer { // MARK: Xbox Controllers public final DriverJoystick driverJoystick = new DriverJoystick(new CommandXboxController(0), drivetrain, shooterSubsystem, intakeSubsystem); public final OperatorJoystick operatorJoystick = new OperatorJoystick(new CommandXboxController(1), drivetrain, intakeSubsystem); - public final DebugJoystick debugJoystick = new DebugJoystick(new CommandXboxController(2), drivetrain, shooterSubsystem); + public final DebugJoystick debugJoystick = new DebugJoystick(new CommandXboxController(2), drivetrain, shooterSubsystem, intakeSubsystem); // MARK: Register Commands public final RegisterCommands registerCommands = new RegisterCommands(intakeSubsystem, shooterSubsystem); diff --git a/src/main/java/frc/robot/commands/NamedCommands.java b/src/main/java/frc/robot/commands/NamedCommands.java deleted file mode 100644 index 657d0286..00000000 --- a/src/main/java/frc/robot/commands/NamedCommands.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.commands; - -public class NamedCommands { - -} diff --git a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java index 60dec356..695b7c8a 100644 --- a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java +++ b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java @@ -2,6 +2,7 @@ import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; import frc.robot.subsystems.mechanisms.climber.ClimberSubsystem; @@ -27,6 +28,14 @@ public RegisterCommands( public void registerCommands(){ NamedCommands.registerCommand("AngleToHub", shooterSubsystem.aimAtHub()); + + NamedCommands.registerCommand("ShootBallsClose", + Commands.run( + () -> { + shooterSubsystem.shooterMotors.runForward(); + } + ).withTimeout(5) + ); } } diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index caec2286..cac2fe87 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; import frc.robot.subsystems.vision.limelight.LimelightConstants; import frc.robot.subsystems.vision.limelight.LimelightHelpers; @@ -17,16 +18,19 @@ public class DebugJoystick { public final CommandXboxController joystick; private final Drive drivetrain; private final ShooterSubsystem shooterSubsystem; + private final IntakeSubsystem intakeSubsystem; private final DoubleEntry shooterTargetTest; public DebugJoystick( CommandXboxController joystick, Drive drivetrain, - ShooterSubsystem shooterSubsystem + ShooterSubsystem shooterSubsystem, + IntakeSubsystem intakeSubsystem ) { this.joystick = joystick; this.drivetrain = drivetrain; this.shooterSubsystem = shooterSubsystem; + this.intakeSubsystem = intakeSubsystem; NetworkTable table = NetworkTableInstance.getDefault().getTable("ShooterSubsystem"); @@ -35,11 +39,22 @@ public DebugJoystick( shooterSubsystem.setDefaultCommand( Commands.run( () -> { - shooterSubsystem.leftShooterMotor.set(joystick.getLeftY()); - shooterSubsystem.rightShooterMotor.set(joystick.getLeftY()); + double leftJoystickValue = Math.abs(joystick.getLeftY()) > 0.05 ? -joystick.getLeftY(): 0.0; + double rightJoystickValue = Math.abs(joystick.getRightY()) > 0.05 ? -joystick.getRightY(): 0.0; + shooterSubsystem.leftShooterMotor.set(leftJoystickValue); + shooterSubsystem.rightShooterMotor.set(leftJoystickValue); + shooterSubsystem.feedMotor.set(rightJoystickValue); }, shooterSubsystem ) ); + + intakeSubsystem.setDefaultCommand( + Commands.run( + () -> { + intakeSubsystem.intakeWheelsMotor.set(joystick.getRightTriggerAxis()); + }, intakeSubsystem + ) + ); } public void configureBindings() { @@ -51,13 +66,7 @@ public void configureBindings() { joystick.y(); - joystick.rightTrigger().whileTrue( - Commands.run( - () -> { - shooterSubsystem.feedMotor.set(Math.min(joystick.getRightTriggerAxis(), 0.5)); - } - ) - ); + // joystick.rightTrigger(); joystick.leftTrigger(); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 98b23f70..09c843b9 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -20,7 +20,7 @@ public void periodic() { Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getPosition()); } - private MotorWrapper intakeWheelsMotor = new MotorWrapper( + public MotorWrapper intakeWheelsMotor = new MotorWrapper( new TalonFX(10), false ); From ac850260133bbeab2f366318a57fe622c0f2c5fa Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Thu, 5 Mar 2026 18:12:25 +0000 Subject: [PATCH 023/102] Fix build to support hyprland arch (bc im cool and use arch and hyprland) --- .vscode/launch.json | 26 ++++- .vscode/settings.json | 2 + build.gradle | 102 +++++++++++++++++++- src/main/java/frc/robot/BuildConstants.java | 10 +- 4 files changed, 132 insertions(+), 8 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 77bee667..13c2122a 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -9,13 +9,35 @@ "name": "Main", "request": "launch", "mainClass": "frc.robot.Main", - "projectName": "roomba" + "projectName": "roomba", + "env": { + "GLFW_PLATFORM": "x11", + "LIBGL_ALWAYS_SOFTWARE": "1", + "LIBGL_DRI3_DISABLE": "1", + "__GLX_VENDOR_LIBRARY_NAME": "mesa", + "MESA_LOADER_DRIVER_OVERRIDE": "llvmpipe", + "GALLIUM_DRIVER": "llvmpipe", + "HALSIM_EXTENSIONS": "${workspaceFolder}/build/jni/release/libhalsim_gui.so", + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release", + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true + "desktop": true, + "env": { + "GLFW_PLATFORM": "x11", + "LIBGL_ALWAYS_SOFTWARE": "1", + "LIBGL_DRI3_DISABLE": "1", + "__GLX_VENDOR_LIBRARY_NAME": "mesa", + "MESA_LOADER_DRIVER_OVERRIDE": "llvmpipe", + "GALLIUM_DRIVER": "llvmpipe", + "HALSIM_EXTENSIONS": "${workspaceFolder}/build/jni/release/libhalsim_gui.so", + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release", + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } }, { "type": "wpilib", diff --git a/.vscode/settings.json b/.vscode/settings.json index 5e6ede86..c2331b7f 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -27,6 +27,8 @@ ], "java.test.defaultConfig": "WPIlibUnitTests", "java.import.gradle.annotationProcessing.enabled": false, + "wpilib.skipSelectSimulateExtension": true, + "wpilib.selectDefaultSimulateExtension": true, "java.completion.favoriteStaticMembers": [ "org.junit.Assert.*", "org.junit.Assume.*", diff --git a/build.gradle b/build.gradle index 732788e6..cf55bbd7 100644 --- a/build.gradle +++ b/build.gradle @@ -88,6 +88,106 @@ test { wpi.sim.addGui().defaultEnabled = true wpi.sim.addDriverstation() +def simSoftwareGl = (project.findProperty("simSoftwareGl") ?: System.getenv("SIM_SOFTWARE_GL"))?.toString()?.toBoolean() +def simGlxVendor = (project.findProperty("simGlxVendor") ?: System.getenv("SIM_GLX_VENDOR"))?.toString() +def waylandSession = System.getenv("WAYLAND_DISPLAY") != null + +tasks.withType(JavaExec).configureEach { + if (!name.startsWith("simulate")) { + return + } + + doFirst { + // Hyprland/Wayland sessions are more reliable when GLFW targets Xwayland. + if (waylandSession && !environment.containsKey("GLFW_PLATFORM")) { + environment("GLFW_PLATFORM", "x11") + } + + if (simSoftwareGl) { + environment("LIBGL_ALWAYS_SOFTWARE", "1") + } + + environment("LIBGL_DRI3_DISABLE", "1") + environment("__GLX_VENDOR_LIBRARY_NAME", "mesa") + environment("MESA_LOADER_DRIVER_OVERRIDE", "llvmpipe") + environment("GALLIUM_DRIVER", "llvmpipe") + + if (simGlxVendor) { + environment("__GLX_VENDOR_LIBRARY_NAME", simGlxVendor) + } + + // Remove empty path entries (e.g. trailing ':') that can break extension loading. + def rawExtensions = (environment["HALSIM_EXTENSIONS"] ?: System.getenv("HALSIM_EXTENSIONS") ?: "").toString() + def cleanedExtensions = rawExtensions.split(":").findAll { !it.trim().isEmpty() }.join(":") + if (rawExtensions != cleanedExtensions) { + environment("HALSIM_EXTENSIONS", cleanedExtensions) + } + } +} + +def patchWpilibJavaSimJson = { File jsonFile -> + if (!jsonFile.exists()) { + return + } + + def parsed = new groovy.json.JsonSlurper().parseText(jsonFile.text) + if (!(parsed instanceof List)) { + return + } + + parsed.each { entry -> + if (!(entry instanceof Map)) { + return + } + + if (!(entry.environment instanceof Map)) { + entry.environment = [:] + } + + // Force X11-backed GLFW on Hyprland/Wayland. + if (waylandSession) { + entry.environment["GLFW_PLATFORM"] = "x11" + } + + // Software rendering avoids broken GLX context creation on this Hyprland setup. + entry.environment["LIBGL_ALWAYS_SOFTWARE"] = "1" + entry.environment["LIBGL_DRI3_DISABLE"] = "1" + entry.environment["__GLX_VENDOR_LIBRARY_NAME"] = "mesa" + entry.environment["MESA_LOADER_DRIVER_OVERRIDE"] = "llvmpipe" + entry.environment["GALLIUM_DRIVER"] = "llvmpipe" + + if (simGlxVendor) { + entry.environment["__GLX_VENDOR_LIBRARY_NAME"] = simGlxVendor + } + + def enabledExtensions = [] + if (entry.extensions instanceof List) { + enabledExtensions = entry.extensions + .findAll { it instanceof Map && it.defaultEnabled && it.libName } + .collect { it.libName.toString() } + } + + if (!enabledExtensions.isEmpty()) { + // Override WPILib extension launcher value to avoid trailing path delimiters. + entry.environment["HALSIM_EXTENSIONS"] = enabledExtensions.join(":") + } + } + + jsonFile.text = groovy.json.JsonOutput.prettyPrint(groovy.json.JsonOutput.toJson(parsed)) + System.lineSeparator() +} + +tasks.matching { it.name == "simulateExternalJavaRelease" }.configureEach { + doLast { + patchWpilibJavaSimJson(file("$buildDir/sim/release_java.json")) + } +} + +tasks.matching { it.name == "simulateExternalJavaDebug" }.configureEach { + doLast { + patchWpilibJavaSimJson(file("$buildDir/sim/debug_java.json")) + } +} + // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') // in order to make them all available at runtime. Also adding the manifest so WPILib // knows where to look for our Robot Class. @@ -119,4 +219,4 @@ gversion { dateFormat = "yyyy-MM-dd HH:mm:ss z" timeZone = "America/Chicago" indent = " " -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 2fa84566..e48d86a2 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 146; - public static final String GIT_SHA = "aed67b9aca058b490c7e477211260716707c116a"; - public static final String GIT_DATE = "2026-03-04 21:35:15 CST"; + public static final int GIT_REVISION = 147; + public static final String GIT_SHA = "f579082753a45b52b0a87e1582e48a21f433bdb3"; + public static final String GIT_DATE = "2026-03-05 11:25:00 CST"; public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-03-05 11:23:54 CST"; - public static final long BUILD_UNIX_TIME = 1772731434178L; + public static final String BUILD_DATE = "2026-03-05 12:09:12 CST"; + public static final long BUILD_UNIX_TIME = 1772734152768L; public static final int DIRTY = 1; private BuildConstants(){} From 3a4807ac747f2ebb2bb6ceab0da5a9a119882573 Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Thu, 5 Mar 2026 18:42:09 +0000 Subject: [PATCH 024/102] Change shooter input method --- .../frc/robot/joysticks/DebugJoystick.java | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index cac2fe87..1cbb3f30 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -2,6 +2,7 @@ import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.networktables.DoubleEntry; import edu.wpi.first.networktables.NetworkTable; @@ -19,7 +20,8 @@ public class DebugJoystick { private final Drive drivetrain; private final ShooterSubsystem shooterSubsystem; private final IntakeSubsystem intakeSubsystem; - private final DoubleEntry shooterTargetTest; + private final DoubleEntry shooterSpeedEntry; + private double shooterSpeed; public DebugJoystick( CommandXboxController joystick, @@ -31,18 +33,28 @@ public DebugJoystick( this.drivetrain = drivetrain; this.shooterSubsystem = shooterSubsystem; this.intakeSubsystem = intakeSubsystem; + this.shooterSpeed = 0.0; NetworkTable table = NetworkTableInstance.getDefault().getTable("ShooterSubsystem"); - shooterTargetTest = table.getDoubleTopic("ShooterAngleEntry").getEntry(1.0); + shooterSpeedEntry = table.getDoubleTopic("ShooterSpeed").getEntry(0.0); + shooterSpeed = shooterSpeedEntry.get(); shooterSubsystem.setDefaultCommand( Commands.run( () -> { + double leftJoystickValue = Math.abs(joystick.getLeftY()) > 0.05 ? -joystick.getLeftY(): 0.0; double rightJoystickValue = Math.abs(joystick.getRightY()) > 0.05 ? -joystick.getRightY(): 0.0; - shooterSubsystem.leftShooterMotor.set(leftJoystickValue); - shooterSubsystem.rightShooterMotor.set(leftJoystickValue); + + double speedChangeAmountPerTick = 0.05; + double change = Math.signum(leftJoystickValue) * speedChangeAmountPerTick; + shooterSpeed = MathUtil.clamp(shooterSpeed + change, 0.0, 1.0); + shooterSpeedEntry.set(shooterSpeed); + + + shooterSubsystem.leftShooterMotor.set(shooterSpeed); + shooterSubsystem.rightShooterMotor.set(shooterSpeed); shooterSubsystem.feedMotor.set(rightJoystickValue); }, shooterSubsystem ) From 546f2ea284cb774cc5b02b400eaa96073bdf5815 Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Thu, 5 Mar 2026 13:57:42 -0600 Subject: [PATCH 025/102] Decrease shooter speed --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/joysticks/DebugJoystick.java | 5 +++-- .../subsystems/mechanisms/intake/IntakeSubsystem.java | 4 ++-- .../mechanisms/shooter/ShooterSubsystem.java | 8 ++++---- 4 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index e48d86a2..c21bf189 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 147; - public static final String GIT_SHA = "f579082753a45b52b0a87e1582e48a21f433bdb3"; - public static final String GIT_DATE = "2026-03-05 11:25:00 CST"; + public static final int GIT_REVISION = 149; + public static final String GIT_SHA = "3a4807ac747f2ebb2bb6ceab0da5a9a119882573"; + public static final String GIT_DATE = "2026-03-05 12:42:09 CST"; public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-03-05 12:09:12 CST"; - public static final long BUILD_UNIX_TIME = 1772734152768L; + public static final String BUILD_DATE = "2026-03-05 13:52:12 CST"; + public static final long BUILD_UNIX_TIME = 1772740332205L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 1cbb3f30..822df877 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -39,6 +39,7 @@ public DebugJoystick( shooterSpeedEntry = table.getDoubleTopic("ShooterSpeed").getEntry(0.0); shooterSpeed = shooterSpeedEntry.get(); + shooterSpeedEntry.set(0.0); shooterSubsystem.setDefaultCommand( Commands.run( @@ -47,9 +48,9 @@ public DebugJoystick( double leftJoystickValue = Math.abs(joystick.getLeftY()) > 0.05 ? -joystick.getLeftY(): 0.0; double rightJoystickValue = Math.abs(joystick.getRightY()) > 0.05 ? -joystick.getRightY(): 0.0; - double speedChangeAmountPerTick = 0.05; + double speedChangeAmountPerTick = 0.005; double change = Math.signum(leftJoystickValue) * speedChangeAmountPerTick; - shooterSpeed = MathUtil.clamp(shooterSpeed + change, 0.0, 1.0); + shooterSpeed = MathUtil.clamp(shooterSpeed + change, -0.1, 1.0); shooterSpeedEntry.set(shooterSpeed); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 09c843b9..2322685b 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -12,7 +12,7 @@ public class IntakeSubsystem extends SubsystemBase { public MotorWrapper angleMotor = new MotorWrapper( - new TalonFX(9), false + new TalonFX(9, "Mechanisms"), false ); @Override @@ -21,7 +21,7 @@ public void periodic() { } public MotorWrapper intakeWheelsMotor = new MotorWrapper( - new TalonFX(10), false + new TalonFX(10, "Mechanisms"), false ); private PositionManager intakePositionManager = new PositionManager( diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 70d0fe44..7b555faa 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -62,12 +62,12 @@ public ShooterSubsystem(Drive drivetrain) { /** Motor controlling the shooter pitch (angle). */ public final MotorWrapper shooterPitchMotor = new MotorWrapper( - new TalonFX(11), + new TalonFX(11, "Mechanisms"), false ); - public MotorWrapper leftShooterMotor = new MotorWrapper(new TalonFX(13), false); - public MotorWrapper rightShooterMotor = new MotorWrapper(new TalonFX(14), true); + public MotorWrapper leftShooterMotor = new MotorWrapper(new TalonFX(13, "Mechanisms"), false); + public MotorWrapper rightShooterMotor = new MotorWrapper(new TalonFX(14, "Mechanisms"), true); public FlywheelPair shooterMotors = new FlywheelPair( leftShooterMotor, @@ -76,7 +76,7 @@ public ShooterSubsystem(Drive drivetrain) { ); public MotorWrapper feedMotor = new MotorWrapper( - new TalonFX(12), + new TalonFX(12, "Mechanisms"), false ); From 2d994fb2cb875adf1dad1d08d3d63ec2d000108a Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Thu, 5 Mar 2026 14:00:56 -0600 Subject: [PATCH 026/102] Add intake button --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/joysticks/DebugJoystick.java | 8 +++++++- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index c21bf189..bd1c964b 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 149; - public static final String GIT_SHA = "3a4807ac747f2ebb2bb6ceab0da5a9a119882573"; - public static final String GIT_DATE = "2026-03-05 12:42:09 CST"; + public static final int GIT_REVISION = 150; + public static final String GIT_SHA = "546f2ea284cb774cc5b02b400eaa96073bdf5815"; + public static final String GIT_DATE = "2026-03-05 13:57:42 CST"; public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-03-05 13:52:12 CST"; - public static final long BUILD_UNIX_TIME = 1772740332205L; + public static final String BUILD_DATE = "2026-03-05 13:59:37 CST"; + public static final long BUILD_UNIX_TIME = 1772740777570L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 822df877..024c5df1 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -77,7 +77,13 @@ public void configureBindings() { joystick.x(); - joystick.y(); + joystick.y().onTrue( + Commands.runOnce( + () -> { + intakeSubsystem.setPosition(0.0); + } + ) + ); // joystick.rightTrigger(); From 6f5d23a1670ae8b3b77df4d8e208a98888d4273c Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Thu, 5 Mar 2026 15:37:48 -0600 Subject: [PATCH 027/102] Decreased acceleration rate. Change speed for feed motor in intake --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/joysticks/DebugJoystick.java | 2 +- src/main/java/frc/robot/joysticks/DriverJoystick.java | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index bd1c964b..46954862 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 150; - public static final String GIT_SHA = "546f2ea284cb774cc5b02b400eaa96073bdf5815"; - public static final String GIT_DATE = "2026-03-05 13:57:42 CST"; + public static final int GIT_REVISION = 151; + public static final String GIT_SHA = "2d994fb2cb875adf1dad1d08d3d63ec2d000108a"; + public static final String GIT_DATE = "2026-03-05 14:00:56 CST"; public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-03-05 13:59:37 CST"; - public static final long BUILD_UNIX_TIME = 1772740777570L; + public static final String BUILD_DATE = "2026-03-05 14:54:52 CST"; + public static final long BUILD_UNIX_TIME = 1772744092838L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 024c5df1..92e36fce 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -48,7 +48,7 @@ public DebugJoystick( double leftJoystickValue = Math.abs(joystick.getLeftY()) > 0.05 ? -joystick.getLeftY(): 0.0; double rightJoystickValue = Math.abs(joystick.getRightY()) > 0.05 ? -joystick.getRightY(): 0.0; - double speedChangeAmountPerTick = 0.005; + double speedChangeAmountPerTick = 0.0025; double change = Math.signum(leftJoystickValue) * speedChangeAmountPerTick; shooterSpeed = MathUtil.clamp(shooterSpeed + change, -0.1, 1.0); shooterSpeedEntry.set(shooterSpeed); diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index 03d2d243..129b28ec 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -50,7 +50,7 @@ public void configureBindings() { joystick.rightTrigger().whileTrue( Commands.run( () -> { - shooterSubsystem.feedMotor.set(0.2); + shooterSubsystem.feedMotor.set(1.0); } ) ).onFalse( From a55a1df892331eb1f054989901b423a71473d1ab Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Thu, 5 Mar 2026 16:42:54 -0600 Subject: [PATCH 028/102] Update autos --- src/main/deploy/pathplanner/autos/1- HOH.auto | 6 +- .../deploy/pathplanner/autos/4 - HNH.auto | 25 ++++ .../pathplanner/autos/Testing Pose Auto.auto | 6 - .../pathplanner/paths/HDH - Part 1.path | 4 +- .../pathplanner/paths/HDH - Part 2.path | 4 +- .../pathplanner/paths/HDH - Part 3.path | 4 +- .../{HOH - Part 2.path => HDH2 - Part 2.path} | 24 ++-- .../pathplanner/paths/HDH2 - Part 3.path | 75 ++++++++++ .../pathplanner/paths/HDHOH - Part 1.path | 2 +- .../pathplanner/paths/HDHOH - Part 2.path | 2 +- .../pathplanner/paths/HDHOH - Part 3.path | 2 +- .../{HOH - Part 3.path => HNH - Part 1.path} | 41 ++---- .../pathplanner/paths/HNH - Part 2.path | 135 ++++++++++++++++++ .../{HOH - Part 1.path => HdH2 - Part 1.path} | 24 ++-- .../pathplanner/paths/Testing Pose Path.path | 18 +-- src/main/deploy/pathplanner/settings.json | 9 +- 16 files changed, 300 insertions(+), 81 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/4 - HNH.auto rename src/main/deploy/pathplanner/paths/{HOH - Part 2.path => HDH2 - Part 2.path} (67%) create mode 100644 src/main/deploy/pathplanner/paths/HDH2 - Part 3.path rename src/main/deploy/pathplanner/paths/{HOH - Part 3.path => HNH - Part 1.path} (51%) create mode 100644 src/main/deploy/pathplanner/paths/HNH - Part 2.path rename src/main/deploy/pathplanner/paths/{HOH - Part 1.path => HdH2 - Part 1.path} (71%) diff --git a/src/main/deploy/pathplanner/autos/1- HOH.auto b/src/main/deploy/pathplanner/autos/1- HOH.auto index c95f0ecb..bd0da273 100644 --- a/src/main/deploy/pathplanner/autos/1- HOH.auto +++ b/src/main/deploy/pathplanner/autos/1- HOH.auto @@ -7,19 +7,19 @@ { "type": "path", "data": { - "pathName": "HOH - Part 1" + "pathName": "HdH2 - Part 1" } }, { "type": "path", "data": { - "pathName": "HOH - Part 2" + "pathName": "HDH2 - Part 2" } }, { "type": "path", "data": { - "pathName": "HOH - Part 3" + "pathName": "HDH2 - Part 3" } } ] diff --git a/src/main/deploy/pathplanner/autos/4 - HNH.auto b/src/main/deploy/pathplanner/autos/4 - HNH.auto new file mode 100644 index 00000000..7a0f4614 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/4 - HNH.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "HNH - Part 1" + } + }, + { + "type": "path", + "data": { + "pathName": "HNH - Part 2" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Testing Pose Auto.auto b/src/main/deploy/pathplanner/autos/Testing Pose Auto.auto index 1483d491..bbd3050b 100644 --- a/src/main/deploy/pathplanner/autos/Testing Pose Auto.auto +++ b/src/main/deploy/pathplanner/autos/Testing Pose Auto.auto @@ -9,12 +9,6 @@ "data": { "pathName": "Testing Pose Path" } - }, - { - "type": "named", - "data": { - "name": "ShootBallsClose" - } } ] } diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 1.path b/src/main/deploy/pathplanner/paths/HDH - Part 1.path index 279ac836..efb2675b 100644 --- a/src/main/deploy/pathplanner/paths/HDH - Part 1.path +++ b/src/main/deploy/pathplanner/paths/HDH - Part 1.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 3.0, "maxAcceleration": 1.5, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 100.0, @@ -45,7 +45,7 @@ "rotation": -90.0 }, "reversed": false, - "folder": "2 - Hub, Depot, Hub (HDH)", + "folder": "1 - Hub, Depot, Hub (HDH)", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 2.path b/src/main/deploy/pathplanner/paths/HDH - Part 2.path index 22a843da..b0ad5da3 100644 --- a/src/main/deploy/pathplanner/paths/HDH - Part 2.path +++ b/src/main/deploy/pathplanner/paths/HDH - Part 2.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 3.0, "maxAcceleration": 1.5, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 100.0, @@ -45,7 +45,7 @@ "rotation": -89.62181552849039 }, "reversed": false, - "folder": "2 - Hub, Depot, Hub (HDH)", + "folder": "1 - Hub, Depot, Hub (HDH)", "idealStartingState": { "velocity": 0, "rotation": -90.0 diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 3.path b/src/main/deploy/pathplanner/paths/HDH - Part 3.path index 92346bf1..6388e837 100644 --- a/src/main/deploy/pathplanner/paths/HDH - Part 3.path +++ b/src/main/deploy/pathplanner/paths/HDH - Part 3.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 3.0, "maxAcceleration": 1.5, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 100.0, @@ -45,7 +45,7 @@ "rotation": 0.0 }, "reversed": false, - "folder": "2 - Hub, Depot, Hub (HDH)", + "folder": "1 - Hub, Depot, Hub (HDH)", "idealStartingState": { "velocity": 0, "rotation": -89.62181552849039 diff --git a/src/main/deploy/pathplanner/paths/HOH - Part 2.path b/src/main/deploy/pathplanner/paths/HDH2 - Part 2.path similarity index 67% rename from src/main/deploy/pathplanner/paths/HOH - Part 2.path rename to src/main/deploy/pathplanner/paths/HDH2 - Part 2.path index f065b3ac..c094d63a 100644 --- a/src/main/deploy/pathplanner/paths/HOH - Part 2.path +++ b/src/main/deploy/pathplanner/paths/HDH2 - Part 2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.495586630003213, - "y": 3.816648148148148 + "x": 2.3739620403321475, + "y": 5.277686832740213 }, "prevControl": null, "nextControl": { - "x": 2.495586630003213, - "y": 1.8166481481481478 + "x": 1.5670225385527887, + "y": 6.364365361803084 }, "isLocked": false, "linkedName": "HOH - 1" }, { "anchor": { - "x": 0.37219777199074133, - "y": 0.6346029369212964 + "x": 0.38351126927639445, + "y": 7.2358600237247925 }, "prevControl": { - "x": 2.3721977719907414, - "y": 0.6346029369212967 + "x": 1.355656635563977, + "y": 7.2358600237247925 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 3.0, "maxAcceleration": 1.5, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 100.0, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 90.0 }, "reversed": false, - "folder": "1 - Hub, Outpost, Hub (HOH)", + "folder": "2- HDH", "idealStartingState": { "velocity": 0, - "rotation": 4.162462895863767 + "rotation": -35.059426966886996 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDH2 - Part 3.path b/src/main/deploy/pathplanner/paths/HDH2 - Part 3.path new file mode 100644 index 00000000..b7b2c7f6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HDH2 - Part 3.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.38351126927639445, + "y": 7.2358600237247925 + }, + "prevControl": null, + "nextControl": { + "x": 0.3835112692763945, + "y": 6.590308422301304 + }, + "isLocked": false, + "linkedName": "HOH - 2" + }, + { + "anchor": { + "x": 0.38351126927639445, + "y": 5.23465005931198 + }, + "prevControl": { + "x": 0.3835112692763944, + "y": 6.052419535551767 + }, + "nextControl": { + "x": 0.3835112692763945, + "y": 4.380533311470473 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.643546856465007, + "y": 4.266322657176749 + }, + "prevControl": { + "x": 2.3094068801898002, + "y": 5.977034400948991 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.077127659574468, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 100.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "2- HDH", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path b/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path index 6dd196af..6c42786c 100644 --- a/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path +++ b/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path @@ -65,7 +65,7 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 3.0, "maxAcceleration": 1.5, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 100.0, diff --git a/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path b/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path index b058375b..384be26b 100644 --- a/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path +++ b/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path @@ -76,7 +76,7 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 3.0, "maxAcceleration": 1.5, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 100.0, diff --git a/src/main/deploy/pathplanner/paths/HDHOH - Part 3.path b/src/main/deploy/pathplanner/paths/HDHOH - Part 3.path index 65b63199..866e6d25 100644 --- a/src/main/deploy/pathplanner/paths/HDHOH - Part 3.path +++ b/src/main/deploy/pathplanner/paths/HDHOH - Part 3.path @@ -44,7 +44,7 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 3.0, "maxAcceleration": 1.5, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 100.0, diff --git a/src/main/deploy/pathplanner/paths/HOH - Part 3.path b/src/main/deploy/pathplanner/paths/HNH - Part 1.path similarity index 51% rename from src/main/deploy/pathplanner/paths/HOH - Part 3.path rename to src/main/deploy/pathplanner/paths/HNH - Part 1.path index 8694c046..59d4944d 100644 --- a/src/main/deploy/pathplanner/paths/HOH - Part 3.path +++ b/src/main/deploy/pathplanner/paths/HNH - Part 1.path @@ -3,48 +3,37 @@ "waypoints": [ { "anchor": { - "x": 0.37219777199074133, - "y": 0.6346029369212964 + "x": 3.641908216307055, + "y": 6.084090469520768 }, "prevControl": null, "nextControl": { - "x": 1.5086383595418482, - "y": 0.8460493881849196 + "x": 3.095264899171878, + "y": 5.963307755446868 }, "isLocked": false, - "linkedName": "HOH - 2" + "linkedName": null }, { "anchor": { - "x": 2.4603841869212966, - "y": 2.5775276331018517 + "x": 2.9902241327342325, + "y": 5.418686721030622 }, "prevControl": { - "x": 2.086335770806592, - "y": 2.3306133708545804 + "x": 3.2445456114622395, + "y": 5.617218969128363 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "HNH - 1" } ], "rotationTargets": [], "constraintZones": [], - "pointTowardsZones": [ - { - "fieldPosition": { - "x": 4.626, - "y": 4.034 - }, - "rotationOffset": 0.0, - "minWaypointRelativePos": 0.4377082011421318, - "maxWaypointRelativePos": 1.0, - "name": "Point Towards Zone" - } - ], + "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 3.0, "maxAcceleration": 1.5, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 100.0, @@ -53,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 34.63187389435466 + "rotation": -40.38523342211157 }, "reversed": false, - "folder": "1 - Hub, Outpost, Hub (HOH)", + "folder": "4 - Hub, Neutral, Hub (HNH)", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HNH - Part 2.path b/src/main/deploy/pathplanner/paths/HNH - Part 2.path new file mode 100644 index 00000000..57dea564 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HNH - Part 2.path @@ -0,0 +1,135 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.9902241327342325, + "y": 5.418686721030622 + }, + "prevControl": null, + "nextControl": { + "x": 3.9902241327342325, + "y": 5.418686721030622 + }, + "isLocked": false, + "linkedName": "HNH - 1" + }, + { + "anchor": { + "x": 4.801654177873641, + "y": 5.418686721030622 + }, + "prevControl": { + "x": 4.395939155303937, + "y": 5.418686721030622 + }, + "nextControl": { + "x": 5.207369200443345, + "y": 5.418686721030622 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.613689205219455, + "y": 7.160545670225385 + }, + "prevControl": { + "x": 7.463060498220641, + "y": 7.666227758007118 + }, + "nextControl": { + "x": 7.721191628014499, + "y": 6.79964467941345 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.463060498220641, + "y": 4.804282325029655 + }, + "prevControl": { + "x": 7.674464556831574, + "y": 5.170716026621937 + }, + "nextControl": { + "x": 7.301672597864768, + "y": 4.524543297746145 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.806144721233689, + "y": 5.632740213523132 + }, + "prevControl": { + "x": 6.444918747871875, + "y": 5.564822805456703 + }, + "nextControl": { + "x": 5.167370694595504, + "y": 5.700657621589561 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.3960854092526698, + "y": 5.084021352313167 + }, + "prevControl": { + "x": 4.073914590747332, + "y": 5.600462633451957 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.7499999999999987, + "rotationDegrees": -45.0 + }, + { + "waypointRelativePos": 2.05, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.932180851063824, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 4.035904255319144, + "rotationDegrees": -45.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 100.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -40.601294645004465 + }, + "reversed": false, + "folder": "4 - Hub, Neutral, Hub (HNH)", + "idealStartingState": { + "velocity": 0, + "rotation": -40.38523342211157 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HOH - Part 1.path b/src/main/deploy/pathplanner/paths/HdH2 - Part 1.path similarity index 71% rename from src/main/deploy/pathplanner/paths/HOH - Part 1.path rename to src/main/deploy/pathplanner/paths/HdH2 - Part 1.path index 3ca48fda..9349af2f 100644 --- a/src/main/deploy/pathplanner/paths/HOH - Part 1.path +++ b/src/main/deploy/pathplanner/paths/HdH2 - Part 1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.6584026786452846, - "y": 3.816648148148148 + "x": 3.6543060498220648, + "y": 4.997947805456701 }, "prevControl": null, "nextControl": { - "x": 2.6584026786452846, - "y": 3.816648148148148 + "x": 2.7699915705321745, + "y": 5.235037351543269 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.495586630003213, - "y": 3.816648148148148 + "x": 2.3739620403321475, + "y": 5.277686832740213 }, "prevControl": { - "x": 3.495586630003213, - "y": 3.816648148148148 + "x": 3.09472357017964, + "y": 4.973614579936882 }, "nextControl": null, "isLocked": false, @@ -37,14 +37,14 @@ "y": 4.034 }, "rotationOffset": 0.0, - "minWaypointRelativePos": 0.0, + "minWaypointRelativePos": 0.17521704814522504, "maxWaypointRelativePos": 1.0, "name": "Point Towards Zone" } ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 3.0, "maxAcceleration": 1.5, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 100.0, @@ -53,10 +53,10 @@ }, "goalEndState": { "velocity": 0, - "rotation": 4.162462895863767 + "rotation": -35.059426966886996 }, "reversed": false, - "folder": "1 - Hub, Outpost, Hub (HOH)", + "folder": "2- HDH", "idealStartingState": { "velocity": 0, "rotation": 0.0 diff --git a/src/main/deploy/pathplanner/paths/Testing Pose Path.path b/src/main/deploy/pathplanner/paths/Testing Pose Path.path index de641bcb..16cf14ff 100644 --- a/src/main/deploy/pathplanner/paths/Testing Pose Path.path +++ b/src/main/deploy/pathplanner/paths/Testing Pose Path.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.589750889679717, - "y": 4.277081850533808 + "x": 3.643546856465007, + "y": 0.3392170818505338 }, "prevControl": null, "nextControl": { - "x": 3.339750889679717, - "y": 4.277081850533808 + "x": 3.393546856465007, + "y": 0.3392170818505338 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.417603795966786, - "y": 4.277081850533808 + "x": 3.008754448398578, + "y": 0.3392170818505338 }, "prevControl": { - "x": 3.667603795966786, - "y": 4.277081850533808 + "x": 3.258754448398578, + "y": 0.3392170818505338 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 3.0, "maxAcceleration": 1.5, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 100.0, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 920cc83e..240de202 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -3,12 +3,13 @@ "robotLength": 0.762, "holonomicMode": true, "pathFolders": [ - "1 - Hub, Outpost, Hub (HOH)", - "2 - Hub, Depot, Hub (HDH)", - "3 - Hub, Depot, Hub, Outpost, Hub (HDHOH)" + "1 - Hub, Depot, Hub (HDH)", + "2- HDH", + "3 - Hub, Depot, Hub, Outpost, Hub (HDHOH)", + "4 - Hub, Neutral, Hub (HNH)" ], "autoFolders": [], - "defaultMaxVel": 1.5, + "defaultMaxVel": 3.0, "defaultMaxAccel": 1.5, "defaultMaxAngVel": 180.0, "defaultMaxAngAccel": 100.0, From 52ffbc844efc3b4bf3694a1b34e2cb66e3670544 Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Thu, 5 Mar 2026 23:19:03 -0600 Subject: [PATCH 029/102] improvements --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/joysticks/DebugJoystick.java | 3 ++- .../subsystems/mechanisms/intake/IntakeSubsystem.java | 4 ++-- .../mechanisms/shooter/ShooterSubsystem.java | 8 ++++---- 4 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 46954862..4f6812c1 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 151; - public static final String GIT_SHA = "2d994fb2cb875adf1dad1d08d3d63ec2d000108a"; - public static final String GIT_DATE = "2026-03-05 14:00:56 CST"; + public static final int GIT_REVISION = 153; + public static final String GIT_SHA = "a55a1df892331eb1f054989901b423a71473d1ab"; + public static final String GIT_DATE = "2026-03-05 16:42:54 CST"; public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-03-05 14:54:52 CST"; - public static final long BUILD_UNIX_TIME = 1772744092838L; + public static final String BUILD_DATE = "2026-03-05 18:00:35 CST"; + public static final long BUILD_UNIX_TIME = 1772755235222L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 92e36fce..c4b3bd6d 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -64,7 +64,8 @@ public DebugJoystick( intakeSubsystem.setDefaultCommand( Commands.run( () -> { - intakeSubsystem.intakeWheelsMotor.set(joystick.getRightTriggerAxis()); + double triggerDirection = joystick.getLeftTriggerAxis() > joystick.getRightTriggerAxis() ? -joystick.getLeftTriggerAxis(): joystick.getRightTriggerAxis(); + intakeSubsystem.intakeWheelsMotor.set(triggerDirection); }, intakeSubsystem ) ); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 2322685b..09c843b9 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -12,7 +12,7 @@ public class IntakeSubsystem extends SubsystemBase { public MotorWrapper angleMotor = new MotorWrapper( - new TalonFX(9, "Mechanisms"), false + new TalonFX(9), false ); @Override @@ -21,7 +21,7 @@ public void periodic() { } public MotorWrapper intakeWheelsMotor = new MotorWrapper( - new TalonFX(10, "Mechanisms"), false + new TalonFX(10), false ); private PositionManager intakePositionManager = new PositionManager( diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 7b555faa..70d0fe44 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -62,12 +62,12 @@ public ShooterSubsystem(Drive drivetrain) { /** Motor controlling the shooter pitch (angle). */ public final MotorWrapper shooterPitchMotor = new MotorWrapper( - new TalonFX(11, "Mechanisms"), + new TalonFX(11), false ); - public MotorWrapper leftShooterMotor = new MotorWrapper(new TalonFX(13, "Mechanisms"), false); - public MotorWrapper rightShooterMotor = new MotorWrapper(new TalonFX(14, "Mechanisms"), true); + public MotorWrapper leftShooterMotor = new MotorWrapper(new TalonFX(13), false); + public MotorWrapper rightShooterMotor = new MotorWrapper(new TalonFX(14), true); public FlywheelPair shooterMotors = new FlywheelPair( leftShooterMotor, @@ -76,7 +76,7 @@ public ShooterSubsystem(Drive drivetrain) { ); public MotorWrapper feedMotor = new MotorWrapper( - new TalonFX(12, "Mechanisms"), + new TalonFX(12), false ); From 026523c23f8eea61fb6b49d99567f05c2a20d924 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Fri, 6 Mar 2026 08:18:21 -0600 Subject: [PATCH 030/102] Some subsystem improvements I guess --- src/main/java/frc/robot/BuildConstants.java | 12 ++++---- .../namedcommands/RegisterCommands.java | 28 +++++++++++++------ .../frc/robot/joysticks/DebugJoystick.java | 28 +++++++++++-------- .../frc/robot/joysticks/DriverJoystick.java | 17 +++-------- .../frc/robot/subsystems/drive/Drive.java | 17 ++++------- .../mechanisms/shooter/ShooterSubsystem.java | 26 ++--------------- 6 files changed, 53 insertions(+), 75 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 4f6812c1..9e6fe798 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,14 +5,14 @@ */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "roomba"; + public static final String MAVEN_NAME = "2026 Roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 153; - public static final String GIT_SHA = "a55a1df892331eb1f054989901b423a71473d1ab"; - public static final String GIT_DATE = "2026-03-05 16:42:54 CST"; + public static final int GIT_REVISION = 154; + public static final String GIT_SHA = "52ffbc844efc3b4bf3694a1b34e2cb66e3670544"; + public static final String GIT_DATE = "2026-03-05 23:19:03 CST"; public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-03-05 18:00:35 CST"; - public static final long BUILD_UNIX_TIME = 1772755235222L; + public static final String BUILD_DATE = "2026-03-05 23:34:42 CST"; + public static final long BUILD_UNIX_TIME = 1772775282662L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java index 695b7c8a..ca6a0506 100644 --- a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java +++ b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java @@ -5,31 +5,24 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; -import frc.robot.subsystems.mechanisms.climber.ClimberSubsystem; -import frc.robot.subsystems.motor.MotorSubsystem; public class RegisterCommands { IntakeSubsystem intakeSubsystem; ShooterSubsystem shooterSubsystem; - // ClimberSubsystem climberSubsystem; - // MotorSubsystem motorSubsystem; public RegisterCommands( IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem - // ClimberSubsystem climberSubsystem, - // MotorSubsystem motorSubsystem ) { this.intakeSubsystem = intakeSubsystem; this.shooterSubsystem = shooterSubsystem; - // this.climberSubsystem = climberSubsystem; - // this.motorSubsystem = motorSubsystem; } public void registerCommands(){ NamedCommands.registerCommand("AngleToHub", shooterSubsystem.aimAtHub()); - NamedCommands.registerCommand("ShootBallsClose", + // MARK: ShootBallsClose + NamedCommands.registerCommand("ShootFullSpeed", Commands.run( () -> { shooterSubsystem.shooterMotors.runForward(); @@ -37,5 +30,22 @@ public void registerCommands(){ ).withTimeout(5) ); + // MARK: IntakeDown + NamedCommands.registerCommand("IntakeDown", + Commands.runOnce( + () -> { + intakeSubsystem.setPosition(0.0); + } + ) + ); + + // MARK: IntakeUp + NamedCommands.registerCommand("IntakeUp", + Commands.runOnce( + () -> { + intakeSubsystem.setPosition(-6.0); + } + ) + ); } } diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index c4b3bd6d..e79486cb 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -2,6 +2,8 @@ import org.littletonrobotics.junction.Logger; +import com.pathplanner.lib.auto.NamedCommands; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.networktables.DoubleEntry; @@ -38,7 +40,6 @@ public DebugJoystick( NetworkTable table = NetworkTableInstance.getDefault().getTable("ShooterSubsystem"); shooterSpeedEntry = table.getDoubleTopic("ShooterSpeed").getEntry(0.0); - shooterSpeed = shooterSpeedEntry.get(); shooterSpeedEntry.set(0.0); shooterSubsystem.setDefaultCommand( @@ -57,6 +58,9 @@ public DebugJoystick( shooterSubsystem.leftShooterMotor.set(shooterSpeed); shooterSubsystem.rightShooterMotor.set(shooterSpeed); shooterSubsystem.feedMotor.set(rightJoystickValue); + + Logger.recordOutput("ShooterSubsystem/ShooterSpeed", shooterSpeed); + Logger.recordOutput("ShooterSubsystem/FeederSpeed", rightJoystickValue); }, shooterSubsystem ) ); @@ -64,27 +68,27 @@ public DebugJoystick( intakeSubsystem.setDefaultCommand( Commands.run( () -> { - double triggerDirection = joystick.getLeftTriggerAxis() > joystick.getRightTriggerAxis() ? -joystick.getLeftTriggerAxis(): joystick.getRightTriggerAxis(); - intakeSubsystem.intakeWheelsMotor.set(triggerDirection); + double triggerSpeed = joystick.getLeftTriggerAxis() > joystick.getRightTriggerAxis() ? -joystick.getLeftTriggerAxis(): joystick.getRightTriggerAxis(); + intakeSubsystem.intakeWheelsMotor.set(triggerSpeed); + + Logger.recordOutput("IntakeSubsystem/WheelSpeed", triggerSpeed); }, intakeSubsystem ) ); } public void configureBindings() { - joystick.a(); + joystick.a().onTrue( + NamedCommands.getCommand("IntakeDown") + ); - joystick.b(); + joystick.b().onTrue( + NamedCommands.getCommand("IntakeUp") + ); joystick.x(); - joystick.y().onTrue( - Commands.runOnce( - () -> { - intakeSubsystem.setPosition(0.0); - } - ) - ); + joystick.y(); // joystick.rightTrigger(); diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index 129b28ec..06f78e28 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -2,15 +2,14 @@ import org.littletonrobotics.junction.Logger; -import edu.wpi.first.math.geometry.Transform3d; +import com.pathplanner.lib.auto.NamedCommands; + import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; -import frc.robot.subsystems.vision.limelight.LimelightConstants; -import frc.robot.subsystems.vision.limelight.LimelightHelpers; public class DriverJoystick { public final CommandXboxController joystick; @@ -76,19 +75,11 @@ public void configureBindings() { ); joystick.rightBumper().onTrue( - Commands.runOnce( - () -> { - shooterSubsystem.toggleShooterMotors(); - } - ) + NamedCommands.getCommand("IntakeDown") ); joystick.leftBumper().onTrue( - Commands.runOnce( - () -> { - intakeSubsystem.setPosition(-6); - } - ) + NamedCommands.getCommand("IntakeUp") ); joystick.povUp(); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index e09a3f7e..15d5f635 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -58,14 +58,14 @@ public Drive(CommandSwerveDrivetrain drivetrain) { public static double MaxSpeed = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed public static double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - // MARK: Field Centric Drive + // MARK: Field Centric public static final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() .withDeadband(MaxSpeed * 0.1) .withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors - // MARK: Field Centric Drive with Heading Control + // MARK: Heading Control public final SwerveRequest.FieldCentricFacingAngle driveFacingHub = new SwerveRequest.FieldCentricFacingAngle() .withHeadingPID(5, 0, 0) @@ -73,7 +73,7 @@ public Drive(CommandSwerveDrivetrain drivetrain) { .withRotationalDeadband(MaxAngularRate * 0.1) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); - // MARK: Robot Centric Drive + // MARK: Robot Centric public static final SwerveRequest.RobotCentric driveRobotCentric = new SwerveRequest.RobotCentric() .withDeadband(MaxSpeed * 0.1) .withRotationalDeadband(MaxAngularRate * 0.1) @@ -84,8 +84,6 @@ public Drive(CommandSwerveDrivetrain drivetrain) { private boolean lockedToHub = false; - public boolean debugMode = false; - // MARK: Periodic Loop @Override public void periodic() { @@ -100,8 +98,6 @@ public void periodic() { Logger.recordOutput("SwerveDrive/Rotation", getPose2d().getRotation()); Logger.recordOutput("SwerveDrive/TargetHubAngle", getAngleToHub()); - - Logger.recordOutput("Debug/Enabled", debugMode); } public Command applyRequest(Supplier request) { @@ -227,6 +223,7 @@ public Command joysticksDefaultCommand(CommandXboxController joystick) { ); } + // MARK: Get Angle to Hub public Rotation2d getAngleToHub() { Pose2d robotPose = getPose2d(); @@ -249,6 +246,7 @@ public Rotation2d getAngleToHub() { return Rotation2d.fromDegrees(rotationAngleDegrees); } + // MARK: Get Distance to Hub public double getDistanceToHub() { Pose2d robotPose = getPose2d(); @@ -266,6 +264,7 @@ public double getDistanceToHub() { return Math.sqrt((xDistance * xDistance) + (yDistance * yDistance)); } + // MARK: Flip Alliance public static Pose2d flipAlliance(Pose2d pose) { if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red) { return new Pose2d( @@ -276,8 +275,4 @@ public static Pose2d flipAlliance(Pose2d pose) { } return pose; } - - public void toggleDebugMode() { - debugMode = !debugMode; - } } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 70d0fe44..eed1fed0 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -11,9 +11,6 @@ import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.networktables.DoubleEntry; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -40,8 +37,7 @@ public class ShooterSubsystem extends SubsystemBase { TreeMap dataPoints = new TreeMap<>(); - private final DoubleEntry shooterSpeedTest; - + // MARK: Constructor /** * Constructs the ShooterSubsystem. * @param drivetrain the swerve drivetrain subsystem (for aiming/coordination) @@ -52,12 +48,6 @@ public ShooterSubsystem(Drive drivetrain) { for (ShooterDataPoint point : ShooterConstants.shooterDataPoints) { dataPoints.put(point.distance, point); } - - NetworkTable table = NetworkTableInstance.getDefault().getTable("ShooterSubsystem"); - - shooterSpeedTest = table.getDoubleTopic("ShooterSpeed").getEntry(0.5); - - shooterSpeedTest.set(0.5); } /** Motor controlling the shooter pitch (angle). */ @@ -103,25 +93,13 @@ public ShooterSubsystem(Drive drivetrain) { // // () -> shooterPitchMotor.getMotor().getPosition().getValueAsDouble() // ); - public boolean shooterFlywheelsEnabled = false; + // public boolean shooterFlywheelsEnabled = false; @Override public void periodic() { Logger.recordOutput("ShooterSubsystem/ThroughBoreAngle", getThroughBorePosition()); // shooterPositionManager.positionTargetManagement(); - if (shooterFlywheelsEnabled) { - shooterMotors.setSpeed(shooterSpeedTest.get()); // Sets speed - shooterMotors.runForward(); // Runs motors - } - else { - shooterMotors.setSpeed(0.0); // Sets speed - shooterMotors.runForward(); // Runs motors - } - } - - public void toggleShooterMotors() { - shooterFlywheelsEnabled = !shooterFlywheelsEnabled; } // --- Commands --- From 9b2d15dd0cf43e58cac8716cbbda9a22ad7ad8a5 Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Fri, 6 Mar 2026 16:18:53 +0000 Subject: [PATCH 031/102] Update driver joystick to run shooter (only at fullspeed currently), rename motorsubsystem>mathsubsystem, add new trajectory function --- src/main/java/frc/robot/BuildConstants.java | 14 +-- src/main/java/frc/robot/RobotContainer.java | 4 +- src/main/java/frc/robot/Tests.java | 6 +- .../frc/robot/joysticks/DebugJoystick.java | 14 +-- .../frc/robot/joysticks/DriverJoystick.java | 106 ++++++++++++------ .../frc/robot/joysticks/OperatorJoystick.java | 47 ++++---- .../MathSubsystem.java} | 28 ++++- .../mechanisms/shooter/FeederState.java | 7 ++ .../mechanisms/shooter/ShooterConstants.java | 12 +- .../mechanisms/shooter/ShooterSubsystem.java | 103 +++++------------ 10 files changed, 187 insertions(+), 154 deletions(-) rename src/main/java/frc/robot/subsystems/{motor/MotorSubsystem.java => math/MathSubsystem.java} (76%) create mode 100644 src/main/java/frc/robot/subsystems/mechanisms/shooter/FeederState.java diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 9e6fe798..c1ad2de1 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,14 +5,14 @@ */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026 Roomba"; + public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 154; - public static final String GIT_SHA = "52ffbc844efc3b4bf3694a1b34e2cb66e3670544"; - public static final String GIT_DATE = "2026-03-05 23:19:03 CST"; - public static final String GIT_BRANCH = "feature/intake-and-shooter"; - public static final String BUILD_DATE = "2026-03-05 23:34:42 CST"; - public static final long BUILD_UNIX_TIME = 1772775282662L; + public static final int GIT_REVISION = 155; + public static final String GIT_SHA = "026523c23f8eea61fb6b49d99567f05c2a20d924"; + public static final String GIT_DATE = "2026-03-06 08:18:21 CST"; + public static final String GIT_BRANCH = "feature/driver-joystick"; + public static final String BUILD_DATE = "2026-03-06 09:03:24 CST"; + public static final long BUILD_UNIX_TIME = 1772809404980L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 16f05eea..bbe32e71 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -56,7 +56,7 @@ public class RobotContainer { // MARK: Xbox Controllers public final DriverJoystick driverJoystick = new DriverJoystick(new CommandXboxController(0), drivetrain, shooterSubsystem, intakeSubsystem); public final OperatorJoystick operatorJoystick = new OperatorJoystick(new CommandXboxController(1), drivetrain, intakeSubsystem); - public final DebugJoystick debugJoystick = new DebugJoystick(new CommandXboxController(2), drivetrain, shooterSubsystem, intakeSubsystem); + // public final DebugJoystick debugJoystick = new DebugJoystick(new CommandXboxController(2), drivetrain, shooterSubsystem, intakeSubsystem); // MARK: Register Commands public final RegisterCommands registerCommands = new RegisterCommands(intakeSubsystem, shooterSubsystem); @@ -91,7 +91,7 @@ public RobotContainer() { private void configureBindings() { driverJoystick.configureBindings(); operatorJoystick.configureBindings(); - debugJoystick.configureBindings(); + // debugJoystick.configureBindings(); // Positive X is forward, Positive Y is left according to WPILib // drivetrain.setDefaultCommand( diff --git a/src/main/java/frc/robot/Tests.java b/src/main/java/frc/robot/Tests.java index 7fc6b71e..a58facc8 100644 --- a/src/main/java/frc/robot/Tests.java +++ b/src/main/java/frc/robot/Tests.java @@ -1,18 +1,18 @@ package frc.robot; +import frc.robot.subsystems.math.MathSubsystem; import frc.robot.subsystems.mechanisms.climber.ClimberSubsystem; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; -import frc.robot.subsystems.motor.MotorSubsystem; public class Tests { IntakeSubsystem intakeSubsystem; ShooterSubsystem shooterSubsystem; ClimberSubsystem climberSubsystem; - MotorSubsystem motorSubsystem; + MathSubsystem motorSubsystem; - public Tests(IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem, ClimberSubsystem climberSubsystem, MotorSubsystem motorSubsystem) { + public Tests(IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem, ClimberSubsystem climberSubsystem, MathSubsystem motorSubsystem) { this.intakeSubsystem = intakeSubsystem; this.shooterSubsystem = shooterSubsystem; this.climberSubsystem = climberSubsystem; diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index e79486cb..87ba2b13 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -22,7 +22,7 @@ public class DebugJoystick { private final Drive drivetrain; private final ShooterSubsystem shooterSubsystem; private final IntakeSubsystem intakeSubsystem; - private final DoubleEntry shooterSpeedEntry; + // private final DoubleEntry shooterSpeedEntry; private double shooterSpeed; public DebugJoystick( @@ -37,10 +37,11 @@ public DebugJoystick( this.intakeSubsystem = intakeSubsystem; this.shooterSpeed = 0.0; - NetworkTable table = NetworkTableInstance.getDefault().getTable("ShooterSubsystem"); + // NetworkTable table = NetworkTableInstance.getDefault().getTable("ShooterSubsystem"); - shooterSpeedEntry = table.getDoubleTopic("ShooterSpeed").getEntry(0.0); - shooterSpeedEntry.set(0.0); + // shooterSpeedEntry = table.getDoubleTopic("ShooterSpeed").getEntry(0.0); + // shooterSpeedEntry.set(0.0); + shooterSubsystem.setDefaultCommand( Commands.run( @@ -52,11 +53,10 @@ public DebugJoystick( double speedChangeAmountPerTick = 0.0025; double change = Math.signum(leftJoystickValue) * speedChangeAmountPerTick; shooterSpeed = MathUtil.clamp(shooterSpeed + change, -0.1, 1.0); - shooterSpeedEntry.set(shooterSpeed); + // shooterSpeedEntry.set(shooterSpeed); - shooterSubsystem.leftShooterMotor.set(shooterSpeed); - shooterSubsystem.rightShooterMotor.set(shooterSpeed); + shooterSubsystem.shooterMotors.setSpeed(shooterSpeed); shooterSubsystem.feedMotor.set(rightJoystickValue); Logger.recordOutput("ShooterSubsystem/ShooterSpeed", shooterSpeed); diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index 06f78e28..a618d9b3 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -4,11 +4,16 @@ import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.networktables.DoubleEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; +import frc.robot.subsystems.mechanisms.shooter.FeederState; +import frc.robot.subsystems.mechanisms.shooter.ShooterDataPoint; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; public class DriverJoystick { @@ -17,6 +22,8 @@ public class DriverJoystick { private final ShooterSubsystem shooterSubsystem; private final IntakeSubsystem intakeSubsystem; + private final DoubleEntry shooterSpeedEntry; + public DriverJoystick( CommandXboxController joystick, Drive drivetrain, @@ -27,6 +34,11 @@ public DriverJoystick( this.drivetrain = drivetrain; this.shooterSubsystem = shooterSubsystem; this.intakeSubsystem = intakeSubsystem; + + NetworkTable table = NetworkTableInstance.getDefault().getTable("DriverJoystick"); + + shooterSpeedEntry = table.getDoubleTopic("ShooterSpeed").getEntry(0.0); + shooterSpeedEntry.set(0.0); } public void configureBindings() { @@ -46,41 +58,67 @@ public void configureBindings() { joystick.y(); - joystick.rightTrigger().whileTrue( - Commands.run( - () -> { - shooterSubsystem.feedMotor.set(1.0); - } - ) - ).onFalse( - Commands.runOnce( - () -> { - shooterSubsystem.feedMotor.set(0.0); - } + final double[] shooterSpeed = {0.0}; + + // MARK: RT - Shooter shoot + joystick.rightTrigger() + .whileTrue( + Commands.startRun( + () -> { + // calculate required shooter speed + ShooterDataPoint shooterDataPoint = shooterSubsystem.calculateShooterValues(shooterSubsystem.shooterUpperLower(), drivetrain.getDistanceToHub()); + + // calculate required speed (0-1) + + shooterSpeed[0] = 1.0; + }, + () -> { + // maintain motor speed + shooterSubsystem.shooterMotors.setSpeed(shooterSpeed[0]); + }, + shooterSubsystem, drivetrain + ) ) - ); - - joystick.leftTrigger().whileTrue( - Commands.run( - () -> { - intakeSubsystem.setIntake(IntakeStates.INTAKE_IN); - } - ) - ).onFalse( - Commands.runOnce( - () -> { - intakeSubsystem.setIntake(IntakeStates.OFF); - } - ) - ); - - joystick.rightBumper().onTrue( - NamedCommands.getCommand("IntakeDown") - ); - - joystick.leftBumper().onTrue( - NamedCommands.getCommand("IntakeUp") - ); + .onFalse( + Commands.run( + () -> { + shooterSpeed[0] = Math.max(0.0, shooterSpeed[0] - 0.05); + shooterSubsystem.shooterMotors.setSpeed(shooterSpeed[0]); + }, + shooterSubsystem + ) + .until(() -> shooterSpeed[0] <= 0.0) + ); + + // MARK: LT - Intake + joystick.leftTrigger() + .whileTrue( + Commands.runEnd( + () -> { + intakeSubsystem.setIntake(IntakeStates.INTAKE_IN); + }, + () -> { + intakeSubsystem.setIntake(IntakeStates.OFF); + }, + intakeSubsystem + ) + ); + + // MARK: RB - Shooter feeder + joystick.rightBumper() + .whileTrue( + Commands.runEnd( + () -> { + shooterSubsystem.setFeeder(FeederState.FEEDER_IN); + }, + () -> { + shooterSubsystem.setFeeder(FeederState.OFF); + }, + shooterSubsystem + ) + ); + + joystick.leftBumper(); joystick.povUp(); diff --git a/src/main/java/frc/robot/joysticks/OperatorJoystick.java b/src/main/java/frc/robot/joysticks/OperatorJoystick.java index cdd27950..45452903 100644 --- a/src/main/java/frc/robot/joysticks/OperatorJoystick.java +++ b/src/main/java/frc/robot/joysticks/OperatorJoystick.java @@ -1,16 +1,21 @@ package frc.robot.joysticks; +import com.pathplanner.lib.auto.NamedCommands; + +import edu.wpi.first.networktables.DoubleEntry; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; +import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; public class OperatorJoystick { public final CommandXboxController joystick; private final Drive drivetrain; private final IntakeSubsystem intakeSubsystem; - + public OperatorJoystick( CommandXboxController joystick, Drive drivetrain, @@ -19,42 +24,34 @@ public OperatorJoystick( this.joystick = joystick; this.drivetrain = drivetrain; this.intakeSubsystem = intakeSubsystem; + + } public void configureBindings() { - joystick.a(); + // MARK: Intake Down + joystick.a().onTrue( + NamedCommands.getCommand("IntakeDown") + ); + + // MARK: Intake Up + joystick.b().onTrue( + NamedCommands.getCommand("IntakeUp") + ); - joystick.b(); joystick.x(); joystick.y(); - // MARK: Intake out - joystick.rightTrigger().onTrue( - Commands.runOnce( - () -> { - intakeSubsystem.setIntake(IntakeStates.INTAKE_OUT); - } - ) - ); + // MARK: Shooter accelerate + joystick.rightTrigger(); // MARK: Intake in - joystick.leftTrigger().onTrue( - Commands.runOnce( - () -> { - intakeSubsystem.setIntake(IntakeStates.INTAKE_IN); - } - ) - ); + joystick.leftTrigger(); - joystick.rightBumper().onTrue( - Commands.runOnce( - () -> { - - } - ) - ); + // MARK: Shooter feeder + joystick.rightBumper(); joystick.leftBumper(); diff --git a/src/main/java/frc/robot/subsystems/motor/MotorSubsystem.java b/src/main/java/frc/robot/subsystems/math/MathSubsystem.java similarity index 76% rename from src/main/java/frc/robot/subsystems/motor/MotorSubsystem.java rename to src/main/java/frc/robot/subsystems/math/MathSubsystem.java index becdccbb..75aad0dd 100644 --- a/src/main/java/frc/robot/subsystems/motor/MotorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/math/MathSubsystem.java @@ -1,5 +1,5 @@ -package frc.robot.subsystems.motor; +package frc.robot.subsystems.math; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -14,7 +14,7 @@ * * In the future this should be moved to WhatTime */ -public class MotorSubsystem extends SubsystemBase { +public class MathSubsystem extends SubsystemBase { /** * Calculates the signed angular difference (in degrees) required to rotate from a * reference heading to face a target described by a 2D position vector. @@ -77,4 +77,28 @@ public double[] calculateTrajectory(double distanceX, double distanceY, double e } + /** + * Calculates trajectory values when the shooter exit angle is provided as input. + * + * The method returns an array: + * [launchSpeed (m/s), enterAngle (degrees)]. + * + * @param distanceX Horizontal distance to the target (meters) + * @param distanceY Vertical distance to the target (meters) + * @param exitAngle Shooter exit angle in degrees (0 = horizontal) + * @return double[] where index 0 is launch speed (m/s), index 1 is enter angle (degrees) + */ + public double[] calculateTrajectoryFromExitAngle(double distanceX, double distanceY, double exitAngle){ + exitAngle = exitAngle * Math.PI / 180; + double enterAngle = Math.atan(2 * distanceY / distanceX - Math.tan(exitAngle)); + + double launchSpeed = Math.sqrt( + ( 9.81 * distanceX * distanceY ) / + ( 2 * Math.cos(exitAngle) * Math.cos(exitAngle) + * ( distanceX * Math.tan(exitAngle) - distanceY ) + )); + + return new double[]{launchSpeed, enterAngle * 180 / Math.PI}; + } + } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/FeederState.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/FeederState.java new file mode 100644 index 00000000..e149a79f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/FeederState.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.mechanisms.shooter; + +public enum FeederState { + FEEDER_IN, + FEEDER_OUT, + OFF; +} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java index 82114ac1..67198a97 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java @@ -26,8 +26,16 @@ public class ShooterConstants { /** Threshold for position manager to consider the shooter "at position". */ public double positionThreshold = 0.02; + public double feederInSpeed = 0.2; + public double feederOutSpeed = -0.05; + + + + /** Height of the hub (target) in meters. */ - // public double hubHeight = 2; + public double hubHeight = 6 / 3.281; // 6 feet to meters + /** Aim above hub in meters */ + public double aimAbove = 1 / 3.281; // 1 foot to meters /** Height of the shooter in meters. */ - // public double shooterHeight = 1; + public double shooterHeight = 20 / 12 / 3.281; // 26 inches to meters } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index eed1fed0..2ae2e7be 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.math.MathSubsystem; @@ -37,6 +38,8 @@ public class ShooterSubsystem extends SubsystemBase { TreeMap dataPoints = new TreeMap<>(); + public ShooterConstants shooterConstants = new ShooterConstants(); + // MARK: Constructor /** * Constructs the ShooterSubsystem. @@ -75,41 +78,28 @@ public ShooterSubsystem(Drive drivetrain) { public final CANcoder shooterThroughBore = new CANcoder(36); - /** - * PositionManager for controlling the shooter pitch motor to a target angle. - * Uses feedback from the shooter IMU. - */ - // public PositionManager shooterPositionManager = new PositionManager( - // shooterPitchMin, - // shooterPitchMax, - // List.of(shooterPitchMotor), - // 0.2, - // shooterPitchHoldSpeed, - // positionThreshold, - // 0.05, - // 0.1, - // () -> getThroughBorePosition() - // // () -> getShooterPitchDeg() - // // () -> shooterPitchMotor.getMotor().getPosition().getValueAsDouble() - // ); - - // public boolean shooterFlywheelsEnabled = false; - - @Override public void periodic() { Logger.recordOutput("ShooterSubsystem/ThroughBoreAngle", getThroughBorePosition()); - // shooterPositionManager.positionTargetManagement(); } - // --- Commands --- - - /** - * Returns a command to pitch the shooter to the specified angle (degrees). - * @param angle target pitch angle in degrees - */ - public void pitchToAngleDeg(double angle) { - // shooterPositionManager.setTarget(angle); + // --- Commands --- \\ + + public void setFeeder(FeederState feederState) { + Logger.recordOutput("ShooterSubsystem/Feeder/State", feederState.toString()); + switch(feederState) { + case FEEDER_IN: + feedMotor.set(shooterConstants.feederInSpeed); + break; + case FEEDER_OUT: + feedMotor.set(shooterConstants.feederOutSpeed); + break; + case OFF: + feedMotor.set(0.0); + break; + default: + break; + } } // MARK: Get Through Bore @@ -117,56 +107,25 @@ public double getThroughBorePosition() { double offset = 0.4; return shooterThroughBore.getAbsolutePosition().getValueAsDouble() + offset; } - - /** - * Returns a command to aim the shooter at the hub. - * Currently a placeholder: should calculate robot position, distance to hub, - * required rotation, and pitch, then command the shooter and drivetrain. - * @return a command that aims the shooter at the hub - */ - public Command aimAtHub() { - return Commands.run( - () -> { - // Calculate the angle and speed needed - ShooterDataPoint hubCalculateDataPoint = calculateShooterValues( - shooterUpperLower(), - drivetrain.getDistanceToHub() - ); - - pitchToAngleDeg(hubCalculateDataPoint.angle); - - shooterMotors.setSpeed(hubCalculateDataPoint.speed); - }, this - ); - } - // --- Sensor feedback --- + public UpperLowerPoint shooterUpperLower() { + // MARK: NEEDS REFACTORING + if (dataPoints.isEmpty()) { - /** - * Gets the shooter pitch motor's position in degrees. - * @return shooter pitch (motor) position in degrees - */ - public double getShooterMotorPitchDeg() { - return shooterPitchMotor.getPosition() * 360; - } + double currentDistance = drivetrain.getDistanceToHub(); + double aimHeight = (6 + 1 - 20 / 12) / 3.281; + + double[] trajectory = (new MathSubsystem()).calculateTrajectoryFromExitAngle(currentDistance, aimHeight, 70); - /** - * Gets the current shooter pitch in degrees from the Pigeon2 IMU. - * @return shooter pitch in degrees (roll axis) - */ - public double getShooterPitchDeg() { - // return shooterPigeon.getRoll().getValueAsDouble(); - return shooterThroughBore.getAbsolutePosition().getValueAsDouble(); - } - public UpperLowerPoint shooterUpperLower() { - if (dataPoints.isEmpty()) { return new UpperLowerPoint( - new ShooterDataPoint(0, 0, 0), - new ShooterDataPoint(0, 0, 0) + new ShooterDataPoint(currentDistance, trajectory[1], trajectory[0]), + new ShooterDataPoint(currentDistance, trajectory[1], trajectory[0]) ); } double currentDistance = drivetrain.getDistanceToHub(); + + Map.Entry lowerEntry = dataPoints.floorEntry(currentDistance); Map.Entry upperEntry = dataPoints.ceilingEntry(currentDistance); From 17aff0653f760e6ed77ac5d6a5db3be2017b352a Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Fri, 6 Mar 2026 16:35:34 +0000 Subject: [PATCH 032/102] Remove unused namedcommand --- simgui-ds.json | 3 ++- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/Robot.java | 1 - src/main/java/frc/robot/RobotContainer.java | 2 +- .../robot/commands/namedcommands/RegisterCommands.java | 2 -- 5 files changed, 8 insertions(+), 10 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index b65e6fb3..1a01ae9b 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -102,5 +102,6 @@ { "guid": "Keyboard2" } - ] + ], + "zeroDisconnectedJoysticks": false } diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index c1ad2de1..4d6c9349 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 155; - public static final String GIT_SHA = "026523c23f8eea61fb6b49d99567f05c2a20d924"; - public static final String GIT_DATE = "2026-03-06 08:18:21 CST"; + public static final int GIT_REVISION = 156; + public static final String GIT_SHA = "9b2d15dd0cf43e58cac8716cbbda9a22ad7ad8a5"; + public static final String GIT_DATE = "2026-03-06 10:18:53 CST"; public static final String GIT_BRANCH = "feature/driver-joystick"; - public static final String BUILD_DATE = "2026-03-06 09:03:24 CST"; - public static final long BUILD_UNIX_TIME = 1772809404980L; + public static final String BUILD_DATE = "2026-03-06 10:22:18 CST"; + public static final long BUILD_UNIX_TIME = 1772814138468L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f0bed600..6e2a8754 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -206,7 +206,6 @@ public void updateNetworkTablesValues() { // NetworkTableInstance.getDefault().getTable("CustomDashboard").getEntry("Pose").setDoubleArray(robotPose); Logger.recordOutput("MatchInfo/TimeRemaining", DriverStation.getMatchTime()); - Logger.recordOutput("ShooterSubsystem/Pitch", robotContainer.shooterSubsystem.getShooterMotorPitchDeg()); //robotField2d.setRobotPose(robotContainer.drivetrain.getState().Pose); // NetworkTablesUtil.put("Main Robot Pose", robotField2d); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bbe32e71..bcfe8e98 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -56,7 +56,7 @@ public class RobotContainer { // MARK: Xbox Controllers public final DriverJoystick driverJoystick = new DriverJoystick(new CommandXboxController(0), drivetrain, shooterSubsystem, intakeSubsystem); public final OperatorJoystick operatorJoystick = new OperatorJoystick(new CommandXboxController(1), drivetrain, intakeSubsystem); - // public final DebugJoystick debugJoystick = new DebugJoystick(new CommandXboxController(2), drivetrain, shooterSubsystem, intakeSubsystem); + public final DebugJoystick debugJoystick = new DebugJoystick(new CommandXboxController(2), drivetrain, shooterSubsystem, intakeSubsystem); // MARK: Register Commands public final RegisterCommands registerCommands = new RegisterCommands(intakeSubsystem, shooterSubsystem); diff --git a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java index ca6a0506..762c7ae3 100644 --- a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java +++ b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java @@ -19,8 +19,6 @@ public RegisterCommands( } public void registerCommands(){ - NamedCommands.registerCommand("AngleToHub", shooterSubsystem.aimAtHub()); - // MARK: ShootBallsClose NamedCommands.registerCommand("ShootFullSpeed", Commands.run( From 6ae8ab2053f62481c8e78aa578eddbf5eed75de8 Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Sat, 7 Mar 2026 16:01:48 +0000 Subject: [PATCH 033/102] Small changes to shooter --- src/main/java/frc/robot/joysticks/DriverJoystick.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index a618d9b3..acc8d877 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -65,12 +65,12 @@ public void configureBindings() { .whileTrue( Commands.startRun( () -> { - // calculate required shooter speed + // calculate required shooter speed and angle ShooterDataPoint shooterDataPoint = shooterSubsystem.calculateShooterValues(shooterSubsystem.shooterUpperLower(), drivetrain.getDistanceToHub()); // calculate required speed (0-1) - shooterSpeed[0] = 1.0; + shooterSpeed[0] = 0.5; }, () -> { // maintain motor speed From a2fae29ce8ab1daf244e008dd1ae42daef111275 Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Sat, 7 Mar 2026 16:46:03 +0000 Subject: [PATCH 034/102] Move from deprecated MotorWrapper to Motor and MotorGroup --- src/main/java/frc/robot/BuildConstants.java | 12 +- src/main/java/frc/robot/Robot.java | 8 +- src/main/java/frc/robot/Tests.java | 5 +- .../namedcommands/RegisterCommands.java | 2 +- .../frc/robot/joysticks/DebugJoystick.java | 16 +-- .../frc/robot/joysticks/DriverJoystick.java | 10 +- .../mechanisms/climber/ClimberSubsystem.java | 117 ------------------ .../mechanisms/intake/IntakeSubsystem.java | 46 +++---- .../mechanisms/shooter/ShooterSubsystem.java | 31 ++--- vendordeps/WhatTime.json | 4 +- 10 files changed, 56 insertions(+), 195 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/mechanisms/climber/ClimberSubsystem.java diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 4d6c9349..8274dc71 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 156; - public static final String GIT_SHA = "9b2d15dd0cf43e58cac8716cbbda9a22ad7ad8a5"; - public static final String GIT_DATE = "2026-03-06 10:18:53 CST"; - public static final String GIT_BRANCH = "feature/driver-joystick"; - public static final String BUILD_DATE = "2026-03-06 10:22:18 CST"; - public static final long BUILD_UNIX_TIME = 1772814138468L; + public static final int GIT_REVISION = 158; + public static final String GIT_SHA = "6ae8ab2053f62481c8e78aa578eddbf5eed75de8"; + public static final String GIT_DATE = "2026-03-07 10:01:48 CST"; + public static final String GIT_BRANCH = "Whattime-refactor"; + public static final String BUILD_DATE = "2026-03-07 10:45:20 CST"; + public static final long BUILD_UNIX_TIME = 1772901920192L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6e2a8754..96d5be2f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -15,7 +15,6 @@ import org.littletonrobotics.junction.wpilog.WPILOGWriter; import com.btwrobotics.WhatTime.frc.DriverStation.MatchTimeManager; -import com.btwrobotics.WhatTime.frc.MotorManagers.MotorBulkActions; import com.btwrobotics.WhatTime.frc.YearlyMethods.Rebuilt.RebuiltHubManager; import com.ctre.phoenix6.HootAutoReplay; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -55,7 +54,6 @@ public class Robot extends LoggedRobot { // Manages rumble for Xbox controller // Start high so it doesn't trigger randomly public double nextRumbleStartTime = 1000; - public MotorBulkActions motorBulkActions = new MotorBulkActions(); @@ -101,10 +99,8 @@ public void robotInit() { currentAlliance = DriverStation.getAlliance(); - motorBulkActions.setNeutralModeBulk(Arrays.asList( - robotContainer.shooterSubsystem.shooterPitchMotor, - robotContainer.intakeSubsystem.angleMotor - ), NeutralModeValue.Brake); + robotContainer.shooterSubsystem.shooterPitchMotor.setNeutralMode(NeutralModeValue.Brake); + robotContainer.intakeSubsystem.angleMotor.setNeutralMode(NeutralModeValue.Brake); // Reset motor speeds to zero. robotContainer.intakeSubsystem.angleMotor.set(0.0); diff --git a/src/main/java/frc/robot/Tests.java b/src/main/java/frc/robot/Tests.java index a58facc8..4ca7513e 100644 --- a/src/main/java/frc/robot/Tests.java +++ b/src/main/java/frc/robot/Tests.java @@ -1,7 +1,6 @@ package frc.robot; import frc.robot.subsystems.math.MathSubsystem; -import frc.robot.subsystems.mechanisms.climber.ClimberSubsystem; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; @@ -9,13 +8,11 @@ public class Tests { IntakeSubsystem intakeSubsystem; ShooterSubsystem shooterSubsystem; - ClimberSubsystem climberSubsystem; MathSubsystem motorSubsystem; - public Tests(IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem, ClimberSubsystem climberSubsystem, MathSubsystem motorSubsystem) { + public Tests(IntakeSubsystem intakeSubsystem, ShooterSubsystem shooterSubsystem, MathSubsystem motorSubsystem) { this.intakeSubsystem = intakeSubsystem; this.shooterSubsystem = shooterSubsystem; - this.climberSubsystem = climberSubsystem; this.motorSubsystem = motorSubsystem; } // Run tests diff --git a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java index 762c7ae3..ca37ad2a 100644 --- a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java +++ b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java @@ -23,7 +23,7 @@ public void registerCommands(){ NamedCommands.registerCommand("ShootFullSpeed", Commands.run( () -> { - shooterSubsystem.shooterMotors.runForward(); + shooterSubsystem.shooterMotors.drive(); } ).withTimeout(5) ); diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 87ba2b13..23126858 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -6,9 +6,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.networktables.DoubleEntry; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; @@ -37,12 +34,6 @@ public DebugJoystick( this.intakeSubsystem = intakeSubsystem; this.shooterSpeed = 0.0; - // NetworkTable table = NetworkTableInstance.getDefault().getTable("ShooterSubsystem"); - - // shooterSpeedEntry = table.getDoubleTopic("ShooterSpeed").getEntry(0.0); - // shooterSpeedEntry.set(0.0); - - shooterSubsystem.setDefaultCommand( Commands.run( () -> { @@ -53,11 +44,10 @@ public DebugJoystick( double speedChangeAmountPerTick = 0.0025; double change = Math.signum(leftJoystickValue) * speedChangeAmountPerTick; shooterSpeed = MathUtil.clamp(shooterSpeed + change, -0.1, 1.0); - // shooterSpeedEntry.set(shooterSpeed); - shooterSubsystem.shooterMotors.setSpeed(shooterSpeed); - shooterSubsystem.feedMotor.set(rightJoystickValue); + shooterSubsystem.shooterMotors.drive(shooterSpeed); + shooterSubsystem.feedMotor.drive(rightJoystickValue); Logger.recordOutput("ShooterSubsystem/ShooterSpeed", shooterSpeed); Logger.recordOutput("ShooterSubsystem/FeederSpeed", rightJoystickValue); @@ -69,7 +59,7 @@ public DebugJoystick( Commands.run( () -> { double triggerSpeed = joystick.getLeftTriggerAxis() > joystick.getRightTriggerAxis() ? -joystick.getLeftTriggerAxis(): joystick.getRightTriggerAxis(); - intakeSubsystem.intakeWheelsMotor.set(triggerSpeed); + intakeSubsystem.intakeWheelsMotor.drive(triggerSpeed); Logger.recordOutput("IntakeSubsystem/WheelSpeed", triggerSpeed); }, intakeSubsystem diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index acc8d877..7820d3a1 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -36,9 +36,11 @@ public DriverJoystick( this.intakeSubsystem = intakeSubsystem; NetworkTable table = NetworkTableInstance.getDefault().getTable("DriverJoystick"); - + shooterSpeedEntry = table.getDoubleTopic("ShooterSpeed").getEntry(0.0); shooterSpeedEntry.set(0.0); + + Logger.recordOutput("DriverJoystick/ShooterSpeed", 0.0); } public void configureBindings() { @@ -73,8 +75,9 @@ public void configureBindings() { shooterSpeed[0] = 0.5; }, () -> { + Logger.recordOutput("DriverJoystick/ShooterSpeed", shooterSpeed[0]); // maintain motor speed - shooterSubsystem.shooterMotors.setSpeed(shooterSpeed[0]); + shooterSubsystem.shooterMotors.drive(shooterSpeed[0]); }, shooterSubsystem, drivetrain ) @@ -83,7 +86,8 @@ public void configureBindings() { Commands.run( () -> { shooterSpeed[0] = Math.max(0.0, shooterSpeed[0] - 0.05); - shooterSubsystem.shooterMotors.setSpeed(shooterSpeed[0]); + shooterSubsystem.shooterMotors.drive(shooterSpeed[0]); + Logger.recordOutput("DriverJoystick/ShooterSpeed", shooterSpeed[0]); }, shooterSubsystem ) diff --git a/src/main/java/frc/robot/subsystems/mechanisms/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/climber/ClimberSubsystem.java deleted file mode 100644 index 3b232675..00000000 --- a/src/main/java/frc/robot/subsystems/mechanisms/climber/ClimberSubsystem.java +++ /dev/null @@ -1,117 +0,0 @@ -package frc.robot.subsystems.mechanisms.climber; - -import com.ctre.phoenix6.hardware.TalonFX; -import com.btwrobotics.WhatTime.frc.FlywheelPair; -import com.btwrobotics.WhatTime.frc.MotorManagers.MotorWrapper; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ClimberSubsystem extends SubsystemBase { - - // TODO: Recalculate all these values - public final double climberUpDownSpeed = 0.5; - public final double climberUpDownSpeedSlow = 0.1; - - public static final double minHeight = 0.0; - public static final double maxHeight = 65.0; - - // TODO: VERY IMPORTANT!!!! Put the correct device ids - public final MotorWrapper climberLeft = new MotorWrapper( - new TalonFX(63), - true - ); - public final MotorWrapper climberRight = new MotorWrapper( - new TalonFX(64), - false - ); - public final FlywheelPair climberPair = new FlywheelPair(climberLeft, climberRight, climberUpDownSpeed); - - - - public double elevatorEncoderOffset = 0.0; - - public void resetClimberEncoder() { - elevatorEncoderOffset = climberLeft.getPosition(); - } - - public double getClimberHeight() { - return climberLeft.getPosition(); - } - - - public Command climberUp() { - return Commands.run( - () -> { - if (Math.abs(climberLeft.getPosition()) <= 65.0) { - climberPair.runForward(); - CommandScheduler.getInstance().cancelAll(); - } else { - climberPair.runForward(0.015); - } - } - ); - } - - public Command climberDown() { - return Commands.run( - () -> { - if (Math.abs(climberLeft.getPosition()) >= 5.0) { - climberPair.runBackward(); - } else { - climberPair.runForward(0.015); - } - } - ); - } - - public Command climberUpManual() { - return Commands.run( - () -> { - climberPair.runForward(); - } - ); - } - - public Command climberDownManual() { - return Commands.run( - () -> { - climberPair.runBackward(); - } - ); - } - - public Command climberUpSlow() { - return Commands.run( - () -> { - if (Math.abs(climberLeft.getPosition()) <= 65.0) { - climberPair.runForward(climberUpDownSpeedSlow); - } else { - climberPair.runForward(0.015); - } - } - ); - } - - public Command climberDownSlow() { - return Commands.run( - () -> { - if (Math.abs(climberLeft.getPosition()) >= 5.0) { - climberPair.runBackward(-climberUpDownSpeedSlow); - } else { - climberPair.runForward(0.015); - } - } - ); - } - - public Command stopClimber() { - return Commands.run( - () -> { - climberPair.runForward(0.015); - } - ); - } -} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 09c843b9..2d2e481f 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -1,44 +1,34 @@ package frc.robot.subsystems.mechanisms.intake; -import java.util.List; - import org.littletonrobotics.junction.Logger; -import com.btwrobotics.WhatTime.frc.MotorManagers.MotorWrapper; -import com.btwrobotics.WhatTime.frc.MotorManagers.PositionManager; -import com.ctre.phoenix6.hardware.TalonFX; +import com.btwrobotics.WhatTime.frc.MotorManagers.Motor; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeSubsystem extends SubsystemBase { - public MotorWrapper angleMotor = new MotorWrapper( - new TalonFX(9), false - ); + public Motor angleMotor = new Motor(9) + .setFree(true); @Override public void periodic() { - Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getPosition()); + Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getCurrentValue()); } - public MotorWrapper intakeWheelsMotor = new MotorWrapper( - new TalonFX(10), false - ); - - private PositionManager intakePositionManager = new PositionManager( - IntakeConstants.minValue, - IntakeConstants.maxValue, - List.of(angleMotor), - 0.2, // Motor Speed - 0.0, // Hold Speed - IntakeConstants.threshold, - 0.1, // Min Speed - 0.1, // Kim Possible (kP) - () -> angleMotor.getPosition() // Use motor encoder for position - ); + public Motor intakeWheelsMotor = new Motor(10) + .setFree(true) + .setMinValue(IntakeConstants.minValue) + .setMaxValue(IntakeConstants.maxValue) + .setMotorSpeed(0.2) + .setHoldSpeed(0.0) + .setThreshold(IntakeConstants.threshold) + .setMinSpeed(0.1) + .setPG(0.1) + .setPositionSupplier(()-> angleMotor.getCurrentValue()); public void setPosition(double targetPosition) { Logger.recordOutput("IntakeSubsystem/SetPosition", targetPosition); - intakePositionManager.setTarget(targetPosition); + intakeWheelsMotor.goTo(targetPosition); } private double intakeSpeed = 0.2; @@ -47,13 +37,13 @@ public void setIntake(IntakeStates intakeState) { Logger.recordOutput("IntakeSubsystem/State", intakeState.toString()); switch (intakeState) { case INTAKE_IN: - intakeWheelsMotor.set(intakeSpeed); + intakeWheelsMotor.drive(intakeSpeed); break; case INTAKE_OUT: - intakeWheelsMotor.set(-intakeSpeed); + intakeWheelsMotor.drive(-intakeSpeed); break; case OFF: - intakeWheelsMotor.set(0); + intakeWheelsMotor.drive(0); break; default: break; diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 2ae2e7be..21fc00ca 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -1,12 +1,15 @@ package frc.robot.subsystems.mechanisms.shooter; +import java.util.Arrays; import java.util.Map; import java.util.TreeMap; import org.littletonrobotics.junction.Logger; -import com.btwrobotics.WhatTime.frc.FlywheelPair; import com.btwrobotics.WhatTime.frc.MotorManagers.MotorWrapper; +import com.btwrobotics.WhatTime.frc.MotorManagers.Motor; +import com.btwrobotics.WhatTime.frc.MotorManagers.MotorGroup; + import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.TalonFX; @@ -54,25 +57,23 @@ public ShooterSubsystem(Drive drivetrain) { } /** Motor controlling the shooter pitch (angle). */ - public final MotorWrapper shooterPitchMotor = new MotorWrapper( - new TalonFX(11), - false - ); - public MotorWrapper leftShooterMotor = new MotorWrapper(new TalonFX(13), false); - public MotorWrapper rightShooterMotor = new MotorWrapper(new TalonFX(14), true); + public final Motor shooterPitchMotor = new Motor(11); - public FlywheelPair shooterMotors = new FlywheelPair( - leftShooterMotor, - rightShooterMotor, - 0.4 - ); + public final Motor leftShooterMotor = new Motor(13) + .setFree(true) + .setMotorSpeed(0.4); + public final Motor rightShooterMotor = new Motor(14, true) + .setFree(true) + .setMotorSpeed(0.4); - public MotorWrapper feedMotor = new MotorWrapper( - new TalonFX(12), - false + public final MotorGroup shooterMotors = new MotorGroup( + Arrays.asList(leftShooterMotor, rightShooterMotor) ); + public Motor feedMotor = new Motor(12) + .setFree(true); + /** IMU sensor for shooter orientation feedback. */ public final Pigeon2 shooterPigeon = new Pigeon2(34); diff --git a/vendordeps/WhatTime.json b/vendordeps/WhatTime.json index 3fda3aec..fe60f17b 100644 --- a/vendordeps/WhatTime.json +++ b/vendordeps/WhatTime.json @@ -1,7 +1,7 @@ { "fileName": "WhatTime.json", "name": "WhatTime", - "version": "v2026.1.16-beta.9", + "version": "v2026.1.16-beta.13", "frcYear": 2026, "uuid": "2C0B26E9-47AA-4C55-B235-640C45ECAD33", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.btwrobotics.whattime.frc", "artifactId": "whattime", - "version": "v2026.1.16-beta.9" + "version": "v2026.1.16-beta.13" } ], "jniDependencies": [], From 5fe22bd3289e5240f281f186bceb40160b51b94d Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Sat, 7 Mar 2026 17:12:14 +0000 Subject: [PATCH 035/102] Update to latest WhatTime --- .../mechanisms/intake/IntakeSubsystem.java | 3 +-- .../mechanisms/shooter/ShooterSubsystem.java | 22 ++++++++----------- vendordeps/WhatTime.json | 4 ++-- 3 files changed, 12 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 2d2e481f..5057f0c0 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -8,7 +8,7 @@ public class IntakeSubsystem extends SubsystemBase { public Motor angleMotor = new Motor(9) - .setFree(true); + .setFree(false); @Override public void periodic() { @@ -16,7 +16,6 @@ public void periodic() { } public Motor intakeWheelsMotor = new Motor(10) - .setFree(true) .setMinValue(IntakeConstants.minValue) .setMaxValue(IntakeConstants.maxValue) .setMotorSpeed(0.2) diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 21fc00ca..a8ab1f88 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -58,21 +58,17 @@ public ShooterSubsystem(Drive drivetrain) { /** Motor controlling the shooter pitch (angle). */ - public final Motor shooterPitchMotor = new Motor(11); + public final Motor shooterPitchMotor = new Motor(11) + .setFree(false); - public final Motor leftShooterMotor = new Motor(13) - .setFree(true) - .setMotorSpeed(0.4); - public final Motor rightShooterMotor = new Motor(14, true) - .setFree(true) - .setMotorSpeed(0.4); + public final Motor leftShooterMotor = new Motor(13); + public final Motor rightShooterMotor = new Motor(14, true); public final MotorGroup shooterMotors = new MotorGroup( Arrays.asList(leftShooterMotor, rightShooterMotor) - ); + ).setMotorSpeed(0.4); - public Motor feedMotor = new Motor(12) - .setFree(true); + public Motor feedMotor = new Motor(12); /** IMU sensor for shooter orientation feedback. */ public final Pigeon2 shooterPigeon = new Pigeon2(34); @@ -90,13 +86,13 @@ public void setFeeder(FeederState feederState) { Logger.recordOutput("ShooterSubsystem/Feeder/State", feederState.toString()); switch(feederState) { case FEEDER_IN: - feedMotor.set(shooterConstants.feederInSpeed); + feedMotor.drive(shooterConstants.feederInSpeed); break; case FEEDER_OUT: - feedMotor.set(shooterConstants.feederOutSpeed); + feedMotor.drive(shooterConstants.feederOutSpeed); break; case OFF: - feedMotor.set(0.0); + feedMotor.drive(0.0); break; default: break; diff --git a/vendordeps/WhatTime.json b/vendordeps/WhatTime.json index fe60f17b..37d4afc9 100644 --- a/vendordeps/WhatTime.json +++ b/vendordeps/WhatTime.json @@ -1,7 +1,7 @@ { "fileName": "WhatTime.json", "name": "WhatTime", - "version": "v2026.1.16-beta.13", + "version": "v2026.1.16-beta.14", "frcYear": 2026, "uuid": "2C0B26E9-47AA-4C55-B235-640C45ECAD33", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.btwrobotics.whattime.frc", "artifactId": "whattime", - "version": "v2026.1.16-beta.13" + "version": "v2026.1.16-beta.14" } ], "jniDependencies": [], From 20ed786213aadd1e9e1ec75e2f42c5905b68431f Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Sat, 7 Mar 2026 17:24:01 +0000 Subject: [PATCH 036/102] Rework inputs --- .../frc/robot/joysticks/DebugJoystick.java | 10 ++++++--- .../frc/robot/joysticks/DriverJoystick.java | 21 ++++++++++++------- .../mechanisms/shooter/ShooterSubsystem.java | 4 +++- 3 files changed, 23 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 23126858..42202d73 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -21,6 +21,7 @@ public class DebugJoystick { private final IntakeSubsystem intakeSubsystem; // private final DoubleEntry shooterSpeedEntry; private double shooterSpeed; + private double shooterPitch; public DebugJoystick( CommandXboxController joystick, @@ -32,7 +33,9 @@ public DebugJoystick( this.drivetrain = drivetrain; this.shooterSubsystem = shooterSubsystem; this.intakeSubsystem = intakeSubsystem; + this.shooterSpeed = 0.0; + this.shooterPitch = 0.0; shooterSubsystem.setDefaultCommand( Commands.run( @@ -41,13 +44,14 @@ public DebugJoystick( double leftJoystickValue = Math.abs(joystick.getLeftY()) > 0.05 ? -joystick.getLeftY(): 0.0; double rightJoystickValue = Math.abs(joystick.getRightY()) > 0.05 ? -joystick.getRightY(): 0.0; - double speedChangeAmountPerTick = 0.0025; - double change = Math.signum(leftJoystickValue) * speedChangeAmountPerTick; + double changeAmountPerTick = 0.0025; + double change = Math.signum(leftJoystickValue) * changeAmountPerTick; shooterSpeed = MathUtil.clamp(shooterSpeed + change, -0.1, 1.0); + shooterPitch = MathUtil.clamp(shooterPitch + Math.signum(rightJoystickValue) * changeAmountPerTick, 0.0 ,0.5); shooterSubsystem.shooterMotors.drive(shooterSpeed); - shooterSubsystem.feedMotor.drive(rightJoystickValue); + shooterSubsystem.shooterPitchMotor.goTo(shooterPitch); Logger.recordOutput("ShooterSubsystem/ShooterSpeed", shooterSpeed); Logger.recordOutput("ShooterSubsystem/FeederSpeed", rightJoystickValue); diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index 7820d3a1..b32d0b0c 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -60,7 +60,7 @@ public void configureBindings() { joystick.y(); - final double[] shooterSpeed = {0.0}; + final double[] shooter = {0.0, 0.25}; // MARK: RT - Shooter shoot joystick.rightTrigger() @@ -72,12 +72,15 @@ public void configureBindings() { // calculate required speed (0-1) - shooterSpeed[0] = 0.5; + shooter[0] = 0.5; + shooter[1] = 0.25; }, () -> { - Logger.recordOutput("DriverJoystick/ShooterSpeed", shooterSpeed[0]); + Logger.recordOutput("DriverJoystick/ShooterSpeed", shooter[0]); + Logger.recordOutput("DriverJoystick/ShooterPitch", shooter[1]); // maintain motor speed - shooterSubsystem.shooterMotors.drive(shooterSpeed[0]); + shooterSubsystem.shooterMotors.drive(shooter[0]); + shooterSubsystem.shooterPitchMotor.goTo(shooter[1]); }, shooterSubsystem, drivetrain ) @@ -85,13 +88,15 @@ public void configureBindings() { .onFalse( Commands.run( () -> { - shooterSpeed[0] = Math.max(0.0, shooterSpeed[0] - 0.05); - shooterSubsystem.shooterMotors.drive(shooterSpeed[0]); - Logger.recordOutput("DriverJoystick/ShooterSpeed", shooterSpeed[0]); + shooter[0] = Math.max(0.0, shooter[0] - 0.05); + shooterSubsystem.shooterMotors.drive(shooter[0]); + shooterSubsystem.shooterPitchMotor.goTo(0.0); + Logger.recordOutput("DriverJoystick/ShooterSpeed", shooter[0]); + Logger.recordOutput("DriverJoystick/ShooterPitch", shooter[1]); }, shooterSubsystem ) - .until(() -> shooterSpeed[0] <= 0.0) + .until(() -> shooter[0] <= 0.0) ); // MARK: LT - Intake diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index a8ab1f88..7e1b8fc5 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -59,7 +59,9 @@ public ShooterSubsystem(Drive drivetrain) { /** Motor controlling the shooter pitch (angle). */ public final Motor shooterPitchMotor = new Motor(11) - .setFree(false); + .setFree(false) + .setRange(0.0, 0.5) + .setMotorSpeed(0.1); public final Motor leftShooterMotor = new Motor(13); public final Motor rightShooterMotor = new Motor(14, true); From e23425f629cf91039e559e7908525606dfa6955a Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Sat, 7 Mar 2026 19:12:55 +0000 Subject: [PATCH 037/102] Make right debug joystick control shooter pitch --- .../frc/robot/joysticks/DebugJoystick.java | 20 ++++++++--- .../frc/robot/joysticks/DriverJoystick.java | 33 ++++++------------- .../mechanisms/shooter/ShooterSubsystem.java | 24 +++----------- 3 files changed, 30 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 42202d73..e0dea5df 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; import frc.robot.subsystems.vision.limelight.LimelightConstants; @@ -19,7 +20,7 @@ public class DebugJoystick { private final Drive drivetrain; private final ShooterSubsystem shooterSubsystem; private final IntakeSubsystem intakeSubsystem; - // private final DoubleEntry shooterSpeedEntry; + private double shooterSpeed; private double shooterPitch; @@ -84,9 +85,20 @@ public void configureBindings() { joystick.y(); - // joystick.rightTrigger(); - - joystick.leftTrigger(); + joystick.rightTrigger(); + + joystick.leftTrigger() + .whileTrue( + Commands.runEnd( + () -> { + intakeSubsystem.setIntake(IntakeStates.INTAKE_IN); + }, + () -> { + intakeSubsystem.setIntake(IntakeStates.OFF); + }, + intakeSubsystem + ) + ); joystick.rightBumper(); diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index b32d0b0c..a2dc8481 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -22,8 +22,6 @@ public class DriverJoystick { private final ShooterSubsystem shooterSubsystem; private final IntakeSubsystem intakeSubsystem; - private final DoubleEntry shooterSpeedEntry; - public DriverJoystick( CommandXboxController joystick, Drive drivetrain, @@ -34,11 +32,6 @@ public DriverJoystick( this.drivetrain = drivetrain; this.shooterSubsystem = shooterSubsystem; this.intakeSubsystem = intakeSubsystem; - - NetworkTable table = NetworkTableInstance.getDefault().getTable("DriverJoystick"); - - shooterSpeedEntry = table.getDoubleTopic("ShooterSpeed").getEntry(0.0); - shooterSpeedEntry.set(0.0); Logger.recordOutput("DriverJoystick/ShooterSpeed", 0.0); } @@ -70,16 +63,18 @@ public void configureBindings() { // calculate required shooter speed and angle ShooterDataPoint shooterDataPoint = shooterSubsystem.calculateShooterValues(shooterSubsystem.shooterUpperLower(), drivetrain.getDistanceToHub()); - // calculate required speed (0-1) + // calculate required speed and angle - shooter[0] = 0.5; - shooter[1] = 0.25; + shooter[0] = 0.5; // speed + shooter[1] = 0.25; // angle (0.5 = 180deg) }, () -> { Logger.recordOutput("DriverJoystick/ShooterSpeed", shooter[0]); Logger.recordOutput("DriverJoystick/ShooterPitch", shooter[1]); + // maintain motor speed shooterSubsystem.shooterMotors.drive(shooter[0]); + shooterSubsystem.shooterFeedMotor.drive(shooter[0]); shooterSubsystem.shooterPitchMotor.goTo(shooter[1]); }, shooterSubsystem, drivetrain @@ -88,9 +83,13 @@ public void configureBindings() { .onFalse( Commands.run( () -> { + shooter[0] = Math.max(0.0, shooter[0] - 0.05); shooterSubsystem.shooterMotors.drive(shooter[0]); + shooterSubsystem.shooterFeedMotor.drive(shooter[0]); + shooterSubsystem.shooterPitchMotor.goTo(0.0); + Logger.recordOutput("DriverJoystick/ShooterSpeed", shooter[0]); Logger.recordOutput("DriverJoystick/ShooterPitch", shooter[1]); }, @@ -113,19 +112,7 @@ public void configureBindings() { ) ); - // MARK: RB - Shooter feeder - joystick.rightBumper() - .whileTrue( - Commands.runEnd( - () -> { - shooterSubsystem.setFeeder(FeederState.FEEDER_IN); - }, - () -> { - shooterSubsystem.setFeeder(FeederState.OFF); - }, - shooterSubsystem - ) - ); + joystick.rightBumper(); joystick.leftBumper(); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 7e1b8fc5..2ad1c053 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -58,9 +58,10 @@ public ShooterSubsystem(Drive drivetrain) { /** Motor controlling the shooter pitch (angle). */ - public final Motor shooterPitchMotor = new Motor(11) + // this is reversed in real life so its inverted in the code + public final Motor shooterPitchMotor = new Motor(11, true) .setFree(false) - .setRange(0.0, 0.5) + .setRange(-0.5, 0) .setMotorSpeed(0.1); public final Motor leftShooterMotor = new Motor(13); @@ -70,7 +71,7 @@ public ShooterSubsystem(Drive drivetrain) { Arrays.asList(leftShooterMotor, rightShooterMotor) ).setMotorSpeed(0.4); - public Motor feedMotor = new Motor(12); + public Motor shooterFeedMotor = new Motor(12); /** IMU sensor for shooter orientation feedback. */ public final Pigeon2 shooterPigeon = new Pigeon2(34); @@ -84,23 +85,6 @@ public void periodic() { // --- Commands --- \\ - public void setFeeder(FeederState feederState) { - Logger.recordOutput("ShooterSubsystem/Feeder/State", feederState.toString()); - switch(feederState) { - case FEEDER_IN: - feedMotor.drive(shooterConstants.feederInSpeed); - break; - case FEEDER_OUT: - feedMotor.drive(shooterConstants.feederOutSpeed); - break; - case OFF: - feedMotor.drive(0.0); - break; - default: - break; - } - } - // MARK: Get Through Bore public double getThroughBorePosition() { double offset = 0.4; From b9b13e5314bd439fe5a57f4b8d3a3fe3090c916b Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Sat, 7 Mar 2026 20:41:16 +0000 Subject: [PATCH 038/102] Begin implementing rpm calculations --- src/main/java/frc/robot/joysticks/DriverJoystick.java | 2 ++ .../subsystems/mechanisms/shooter/ShooterSubsystem.java | 7 ++++++- vendordeps/WhatTime.json | 4 ++-- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index a2dc8481..a37fb140 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -65,6 +65,8 @@ public void configureBindings() { // calculate required speed and angle + double rpm = shooterSubsystem.getRequiredRPM(shooterDataPoint); + shooter[0] = 0.5; // speed shooter[1] = 0.25; // angle (0.5 = 180deg) }, diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 2ad1c053..9539b279 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -96,7 +96,7 @@ public UpperLowerPoint shooterUpperLower() { if (dataPoints.isEmpty()) { double currentDistance = drivetrain.getDistanceToHub(); - double aimHeight = (6 + 1 - 20 / 12) / 3.281; + double aimHeight = (6 - 20 / 12) / 3.281; double[] trajectory = (new MathSubsystem()).calculateTrajectoryFromExitAngle(currentDistance, aimHeight, 70); @@ -135,4 +135,9 @@ public ShooterDataPoint calculateShooterValues(UpperLowerPoint upperLowerPoint, return new ShooterDataPoint(currentDistance, estimatedAngle, estimatedSpeed); } + + public double getRequiredRPM(ShooterDataPoint shooterDataPoint){ + double vWheel = 2 * shooterDataPoint.speed; + return vWheel * 60 / (Math.PI * 4 * 0.0254); // get rpm required for wheel with diameter of 4 inches + } } diff --git a/vendordeps/WhatTime.json b/vendordeps/WhatTime.json index 37d4afc9..9fee9a0c 100644 --- a/vendordeps/WhatTime.json +++ b/vendordeps/WhatTime.json @@ -1,7 +1,7 @@ { "fileName": "WhatTime.json", "name": "WhatTime", - "version": "v2026.1.16-beta.14", + "version": "v2026.1.16-beta.15", "frcYear": 2026, "uuid": "2C0B26E9-47AA-4C55-B235-640C45ECAD33", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.btwrobotics.whattime.frc", "artifactId": "whattime", - "version": "v2026.1.16-beta.14" + "version": "v2026.1.16-beta.15" } ], "jniDependencies": [], From 87a374b06b0bc4c2b3503bd0faeacf496030bc09 Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Sat, 7 Mar 2026 20:41:33 +0000 Subject: [PATCH 039/102] Remove unused import --- src/main/java/frc/robot/joysticks/DriverJoystick.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index a37fb140..805e36cf 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -12,7 +12,6 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; -import frc.robot.subsystems.mechanisms.shooter.FeederState; import frc.robot.subsystems.mechanisms.shooter.ShooterDataPoint; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; From 8467f22af087c6a5a9451783d9b3d230186a6022 Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Sat, 7 Mar 2026 21:13:35 +0000 Subject: [PATCH 040/102] Add shooter calculations --- .../frc/robot/joysticks/DriverJoystick.java | 48 ++++++++++++------- .../mechanisms/shooter/ShooterSubsystem.java | 36 +++++++------- 2 files changed, 48 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index 805e36cf..f29353bb 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -52,31 +52,40 @@ public void configureBindings() { joystick.y(); - final double[] shooter = {0.0, 0.25}; + final ShooterDataPoint[] saveShooterDataPoint = {new ShooterDataPoint(0.0, 0.0, 0.0)}; // MARK: RT - Shooter shoot joystick.rightTrigger() .whileTrue( Commands.startRun( () -> { - // calculate required shooter speed and angle - ShooterDataPoint shooterDataPoint = shooterSubsystem.calculateShooterValues(shooterSubsystem.shooterUpperLower(), drivetrain.getDistanceToHub()); - - // calculate required speed and angle + ShooterDataPoint shooterDataPoint = saveShooterDataPoint[0]; + + shooterDataPoint = shooterSubsystem.calculateShooterValues( + shooterSubsystem.shooterUpperLower(), + drivetrain.getDistanceToHub() + ); double rpm = shooterSubsystem.getRequiredRPM(shooterDataPoint); - shooter[0] = 0.5; // speed - shooter[1] = 0.25; // angle (0.5 = 180deg) + double maxRPM = 1000; // MARK: Populate max rpm + + shooterDataPoint.speed = rpm / maxRPM; + shooterDataPoint.angle = shooterDataPoint.angle / 180; // angle (0.5 = 180deg) + + saveShooterDataPoint[0] = shooterDataPoint; }, () -> { - Logger.recordOutput("DriverJoystick/ShooterSpeed", shooter[0]); - Logger.recordOutput("DriverJoystick/ShooterPitch", shooter[1]); + ShooterDataPoint shooterDataPoint = saveShooterDataPoint[0]; + Logger.recordOutput("DriverJoystick/ShooterSpeed", shooterDataPoint.speed); + Logger.recordOutput("DriverJoystick/ShooterPitch", shooterDataPoint.speed); // maintain motor speed - shooterSubsystem.shooterMotors.drive(shooter[0]); - shooterSubsystem.shooterFeedMotor.drive(shooter[0]); - shooterSubsystem.shooterPitchMotor.goTo(shooter[1]); + shooterSubsystem.shooterMotors.drive(shooterDataPoint.speed); + shooterSubsystem.shooterFeedMotor.drive(shooterDataPoint.speed); + shooterSubsystem.shooterPitchMotor.goTo(shooterDataPoint.angle); + + saveShooterDataPoint[0] = shooterDataPoint; }, shooterSubsystem, drivetrain ) @@ -84,19 +93,22 @@ public void configureBindings() { .onFalse( Commands.run( () -> { + ShooterDataPoint shooterDataPoint = saveShooterDataPoint[0]; - shooter[0] = Math.max(0.0, shooter[0] - 0.05); - shooterSubsystem.shooterMotors.drive(shooter[0]); - shooterSubsystem.shooterFeedMotor.drive(shooter[0]); + shooterDataPoint.speed = Math.max(0.0, shooterDataPoint.speed - 0.05); + shooterSubsystem.shooterMotors.drive(shooterDataPoint.speed); + shooterSubsystem.shooterFeedMotor.drive(shooterDataPoint.speed); shooterSubsystem.shooterPitchMotor.goTo(0.0); - Logger.recordOutput("DriverJoystick/ShooterSpeed", shooter[0]); - Logger.recordOutput("DriverJoystick/ShooterPitch", shooter[1]); + Logger.recordOutput("DriverJoystick/ShooterSpeed", shooterDataPoint.speed); + Logger.recordOutput("DriverJoystick/ShooterPitch", shooterDataPoint.angle); + + saveShooterDataPoint[0] = shooterDataPoint; }, shooterSubsystem ) - .until(() -> shooter[0] <= 0.0) + .until(() -> saveShooterDataPoint[0].speed <= 0.0) ); // MARK: LT - Intake diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 9539b279..6fc5ccf9 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -93,7 +93,7 @@ public double getThroughBorePosition() { public UpperLowerPoint shooterUpperLower() { // MARK: NEEDS REFACTORING - if (dataPoints.isEmpty()) { + // if (dataPoints.isEmpty()) { double currentDistance = drivetrain.getDistanceToHub(); double aimHeight = (6 - 20 / 12) / 3.281; @@ -105,35 +105,35 @@ public UpperLowerPoint shooterUpperLower() { new ShooterDataPoint(currentDistance, trajectory[1], trajectory[0]), new ShooterDataPoint(currentDistance, trajectory[1], trajectory[0]) ); - } - double currentDistance = drivetrain.getDistanceToHub(); + // } + // double currentDistance = drivetrain.getDistanceToHub(); - Map.Entry lowerEntry = dataPoints.floorEntry(currentDistance); - Map.Entry upperEntry = dataPoints.ceilingEntry(currentDistance); + // Map.Entry lowerEntry = dataPoints.floorEntry(currentDistance); + // Map.Entry upperEntry = dataPoints.ceilingEntry(currentDistance); - // Handle out-of-range cases by clamping to the nearest point - ShooterDataPoint lower = (lowerEntry != null) ? lowerEntry.getValue() : upperEntry.getValue(); - ShooterDataPoint upper = (upperEntry != null) ? upperEntry.getValue() : lowerEntry.getValue(); + // // Handle out-of-range cases by clamping to the nearest point + // ShooterDataPoint lower = (lowerEntry != null) ? lowerEntry.getValue() : upperEntry.getValue(); + // ShooterDataPoint upper = (upperEntry != null) ? upperEntry.getValue() : lowerEntry.getValue(); - return new UpperLowerPoint(upper, lower); + // return new UpperLowerPoint(upper, lower); } public ShooterDataPoint calculateShooterValues(UpperLowerPoint upperLowerPoint, double currentDistance) { - double valueRange = Math.abs(upperLowerPoint.getUpperDistance() - upperLowerPoint.getLowerDistance()); - if (valueRange == 0) { + // double valueRange = Math.abs(upperLowerPoint.getUpperDistance() - upperLowerPoint.getLowerDistance()); + // if (valueRange == 0) { return new ShooterDataPoint(currentDistance, upperLowerPoint.getUpperAngle(), upperLowerPoint.getUpperSpeed()); - } + // } - double scaledValue = currentDistance - Math.min(upperLowerPoint.getUpperDistance(), upperLowerPoint.getLowerDistance()); + // double scaledValue = currentDistance - Math.min(upperLowerPoint.getUpperDistance(), upperLowerPoint.getLowerDistance()); - double interpolationFactor = scaledValue/valueRange; + // double interpolationFactor = scaledValue/valueRange; - // Interpolate the angle and speed between the data points - double estimatedAngle = upperLowerPoint.getLowerAngle() + interpolationFactor * (upperLowerPoint.getUpperAngle() - upperLowerPoint.getLowerAngle()); - double estimatedSpeed = upperLowerPoint.getLowerSpeed() + interpolationFactor * (upperLowerPoint.getUpperSpeed() - upperLowerPoint.getLowerSpeed()); + // // Interpolate the angle and speed between the data points + // double estimatedAngle = upperLowerPoint.getLowerAngle() + interpolationFactor * (upperLowerPoint.getUpperAngle() - upperLowerPoint.getLowerAngle()); + // double estimatedSpeed = upperLowerPoint.getLowerSpeed() + interpolationFactor * (upperLowerPoint.getUpperSpeed() - upperLowerPoint.getLowerSpeed()); - return new ShooterDataPoint(currentDistance, estimatedAngle, estimatedSpeed); + // return new ShooterDataPoint(currentDistance, estimatedAngle, estimatedSpeed); } public double getRequiredRPM(ShooterDataPoint shooterDataPoint){ From cc91fe8a7de86ab6fa915c2941825176407f61d5 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Wed, 11 Mar 2026 15:32:23 -0500 Subject: [PATCH 041/102] Updates to logging. Changed WhatTime version. Removed unused code. --- src/main/java/frc/robot/BuildConstants.java | 12 ++-- src/main/java/frc/robot/Robot.java | 62 ++++++++----------- src/main/java/frc/robot/RobotContainer.java | 2 - .../frc/robot/joysticks/DriverJoystick.java | 5 -- .../frc/robot/joysticks/OperatorJoystick.java | 5 -- .../mechanisms/shooter/ShooterSubsystem.java | 5 -- .../vision/limelight/LimelightSubsystem.java | 8 +++ vendordeps/WhatTime.json | 4 +- 8 files changed, 41 insertions(+), 62 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 8274dc71..17ccac0c 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,14 +5,14 @@ */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "roomba"; + public static final String MAVEN_NAME = "2026 Roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 158; - public static final String GIT_SHA = "6ae8ab2053f62481c8e78aa578eddbf5eed75de8"; - public static final String GIT_DATE = "2026-03-07 10:01:48 CST"; + public static final int GIT_REVISION = 165; + public static final String GIT_SHA = "8467f22af087c6a5a9451783d9b3d230186a6022"; + public static final String GIT_DATE = "2026-03-07 15:13:35 CST"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-07 10:45:20 CST"; - public static final long BUILD_UNIX_TIME = 1772901920192L; + public static final String BUILD_DATE = "2026-03-09 17:42:38 CDT"; + public static final long BUILD_UNIX_TIME = 1773096158530L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 96d5be2f..f7cc8301 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,7 +4,6 @@ package frc.robot; -import java.util.Arrays; import java.util.Optional; import org.littletonrobotics.junction.LogFileUtil; @@ -22,8 +21,6 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.GenericHID.RumbleType; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.AdvantageKit.AdvantageKitConstants; @@ -51,12 +48,6 @@ public class Robot extends LoggedRobot { // The current alliance for the robot public Optional currentAlliance; - // Manages rumble for Xbox controller - // Start high so it doesn't trigger randomly - public double nextRumbleStartTime = 1000; - - - // MARK: Hub Manager public MatchTimeManager matchTimeManager = new MatchTimeManager(); public RebuiltHubManager rebuiltHubManager = new RebuiltHubManager(matchTimeManager); @@ -65,6 +56,7 @@ public Robot() { robotContainer = new RobotContainer(); } + // MARK: Robot Init @Override public void robotInit() { // Configure logging for AdvantageKit @@ -110,6 +102,7 @@ public void robotInit() { Logger.recordOutput("FieldInfo/CurrentAlliance", currentAlliance.toString()); } + // MARK: Robot Periodic @Override public void robotPeriodic() { pdp.clearStickyFaults(); @@ -118,32 +111,22 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); - matchTimeRemainingSeconds = DriverStation.getMatchTime(); - matchTimeElapsedSeconds = 160 - matchTimeRemainingSeconds; - - if (matchTimeElapsedSeconds - nextRumbleStartTime >= 0 && matchTimeElapsedSeconds - nextRumbleStartTime <= 2) { - robotContainer.driverJoystick.joystick.setRumble(RumbleType.kBothRumble, 1.0); - robotContainer.operatorJoystick.joystick.setRumble(RumbleType.kBothRumble, 1.0); - robotContainer.debugJoystick.joystick.setRumble(RumbleType.kBothRumble, 1.0); - } - else { - robotContainer.driverJoystick.joystick.setRumble(RumbleType.kBothRumble, 0.0); - robotContainer.operatorJoystick.joystick.setRumble(RumbleType.kBothRumble, 0.0); - robotContainer.debugJoystick.joystick.setRumble(RumbleType.kBothRumble, 0.0); - } - - updateNetworkTablesValues(); + logDriveStationValues(); } + // MARK: Disabled Init @Override public void disabledInit() {} + // MARK: Disabled Periodic @Override public void disabledPeriodic() {} + // MARK: Disabled Exit @Override public void disabledExit() {} + // MARK: Autonomous Init @Override public void autonomousInit() { m_autonomousCommand = robotContainer.getAutonomousCommand(); @@ -153,12 +136,15 @@ public void autonomousInit() { } } + // MARK: Autonomous Periodic @Override public void autonomousPeriodic() {} + // MARK: Autonomous Exit @Override public void autonomousExit() {} + // MARK: Teleop Init @Override public void teleopInit() { if (m_autonomousCommand != null) { @@ -166,6 +152,7 @@ public void teleopInit() { } } + // MARK: Teleop Periodic @Override public void teleopPeriodic() { // NetworkTablesUtil.put("Hub is Active", rebuiltHubManager.hubIsActive()); @@ -174,36 +161,37 @@ public void teleopPeriodic() { Logger.recordOutput("RebuiltHubManager/InactiveFirst", rebuiltHubManager.getInactiveFirstAlliance().toString()); } + // MARK: Teleop Exit @Override public void teleopExit() {} + // MARK: Test Init @Override public void testInit() { CommandScheduler.getInstance().cancelAll(); } + // MARK: Test Periodic @Override public void testPeriodic() {} + // MARK: Test Exit @Override public void testExit() {} + // MARK: Simulation Periodic @Override public void simulationPeriodic() {} - Field2d robotField2d = new Field2d(); - - Field2d limelight4Field2d = new Field2d(); - Field2d limelight2Field2d = new Field2d(); - - public void updateNetworkTablesValues() { - // MARK: use limelight to calculate this - // double[] robotPose = NetworkTableInstance.getDefault().getTable("Pose").getEntry("robotPose").getDoubleArray(new double[]{0.0,0.0,0.0}); - // NetworkTableInstance.getDefault().getTable("CustomDashboard").getEntry("Pose").setDoubleArray(robotPose); - - Logger.recordOutput("MatchInfo/TimeRemaining", DriverStation.getMatchTime()); - //robotField2d.setRobotPose(robotContainer.drivetrain.getState().Pose); - // NetworkTablesUtil.put("Main Robot Pose", robotField2d); + private void logDriveStationValues() { + Logger.recordOutput("DriverStation/GameSpecificMessage", DriverStation.getGameSpecificMessage()); + Logger.recordOutput("DriverStation/MatchTime", DriverStation.getMatchTime()); + Logger.recordOutput("DriverStation/MatchType", DriverStation.getMatchType()); + Logger.recordOutput("DriverStation/MatchNumber", DriverStation.getMatchNumber()); + Logger.recordOutput("DriverStation/IsAutonomous", DriverStation.isAutonomous()); + Logger.recordOutput("DriverStation/AutonomousEnabled", DriverStation.isAutonomousEnabled()); + Logger.recordOutput("DriverStation/IsEnabled", DriverStation.isEnabled()); + Logger.recordOutput("DriverStation/IsEStopped", DriverStation.isEStopped()); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bcfe8e98..56744a49 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -48,7 +48,6 @@ public class RobotContainer { // LimelightSubsystem limelight2Subsystem = new LimelightSubsystem(drivetrain, "limelight-two"); // MARK: Subsystems - // public final ClimberSubsystem climberSubsystem = new ClimberSubsystem(); public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); // public final MotorSubsystem motorSubsystem = new MotorSubsystem(); public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(drivetrain); @@ -61,7 +60,6 @@ public class RobotContainer { // MARK: Register Commands public final RegisterCommands registerCommands = new RegisterCommands(intakeSubsystem, shooterSubsystem); - // MARK: Tests // public final Tests tests = new Tests(intakeSubsystem, shooterSubsystem, climberSubsystem, motorSubsystem); diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index f29353bb..10e0b170 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -2,11 +2,6 @@ import org.littletonrobotics.junction.Logger; -import com.pathplanner.lib.auto.NamedCommands; - -import edu.wpi.first.networktables.DoubleEntry; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; diff --git a/src/main/java/frc/robot/joysticks/OperatorJoystick.java b/src/main/java/frc/robot/joysticks/OperatorJoystick.java index 45452903..b1c2eb14 100644 --- a/src/main/java/frc/robot/joysticks/OperatorJoystick.java +++ b/src/main/java/frc/robot/joysticks/OperatorJoystick.java @@ -2,14 +2,9 @@ import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.networktables.DoubleEntry; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; -import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; public class OperatorJoystick { public final CommandXboxController joystick; diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 6fc5ccf9..b883a1e2 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -1,21 +1,16 @@ package frc.robot.subsystems.mechanisms.shooter; import java.util.Arrays; -import java.util.Map; import java.util.TreeMap; import org.littletonrobotics.junction.Logger; -import com.btwrobotics.WhatTime.frc.MotorManagers.MotorWrapper; import com.btwrobotics.WhatTime.frc.MotorManagers.Motor; import com.btwrobotics.WhatTime.frc.MotorManagers.MotorGroup; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; -import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.math.MathSubsystem; diff --git a/src/main/java/frc/robot/subsystems/vision/limelight/LimelightSubsystem.java b/src/main/java/frc/robot/subsystems/vision/limelight/LimelightSubsystem.java index 09c4e08d..e269a97e 100644 --- a/src/main/java/frc/robot/subsystems/vision/limelight/LimelightSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/limelight/LimelightSubsystem.java @@ -69,6 +69,9 @@ public LimelightSubsystem( Field2d limelightField2d = new Field2d(); + private int totalLimelightEstimates = 0; + private int estimatesAddedToQuest = 0; + /** * Periodic update called by the scheduler. Adds a vision odometry measurement each cycle. *

Delegates to {@link #addOdometryMeasurement()} to perform the actual read/filter/submit @@ -132,8 +135,13 @@ public void addOdometryMeasurement() { // Add measurement to drivetrain pose estimator drivetrain.addVisionMeasurement(transformedPose, estimate.timestampSeconds, LimelightConstants.calculateQuestUpdateStdDevs(estimate)); + totalLimelightEstimates++; + Logger.recordOutput("Limelight/TotalEstimates", totalLimelightEstimates); + // Add measurement to QuestNav pose estimator if enabled if (QuestNavConstants.USE_LIMELIGHT_FOR_VISION_MEASUREMENTS) { + estimatesAddedToQuest++; + Logger.recordOutput("QuestNav/LimelightEstimates", estimatesAddedToQuest); questNavSubsystem.addVisionMeasurement(transformedPose, estimate.timestampSeconds, LimelightConstants.calculateQuestUpdateStdDevs(estimate), estimate); } } diff --git a/vendordeps/WhatTime.json b/vendordeps/WhatTime.json index 9fee9a0c..7911b7d1 100644 --- a/vendordeps/WhatTime.json +++ b/vendordeps/WhatTime.json @@ -1,7 +1,7 @@ { "fileName": "WhatTime.json", "name": "WhatTime", - "version": "v2026.1.16-beta.15", + "version": "v2026.2.1-beta.1", "frcYear": 2026, "uuid": "2C0B26E9-47AA-4C55-B235-640C45ECAD33", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.btwrobotics.whattime.frc", "artifactId": "whattime", - "version": "v2026.1.16-beta.15" + "version": "v2026.2.1-beta.1" } ], "jniDependencies": [], From ced371b7da68c983389924811c84fac52cdffcea Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Wed, 11 Mar 2026 17:38:02 -0500 Subject: [PATCH 042/102] Organized some code. Removed more unused stuff. Added feeder subsystem. MathSubsystem no longer extends SubsystemBase. --- src/main/java/frc/robot/Robot.java | 2 + src/main/java/frc/robot/RobotContainer.java | 48 +++++++------------ .../namedcommands/RegisterCommands.java | 12 ++++- .../frc/robot/joysticks/DriverJoystick.java | 10 ++-- .../robot/subsystems/math/MathSubsystem.java | 4 +- .../mechanisms/feeder/FeederConstants.java | 5 ++ .../mechanisms/feeder/FeederSubsystem.java | 46 ++++++++++++++++++ .../mechanisms/intake/IntakeSubsystem.java | 13 +++-- .../mechanisms/shooter/ShooterSubsystem.java | 2 - 9 files changed, 98 insertions(+), 44 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java create mode 100644 src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f7cc8301..4a2f6956 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -89,6 +89,8 @@ public void robotInit() { // Start AdvantageKit logging Logger.start(); + DriverStation.silenceJoystickConnectionWarning(true); + currentAlliance = DriverStation.getAlliance(); robotContainer.shooterSubsystem.shooterPitchMotor.setNeutralMode(NeutralModeValue.Brake); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 56744a49..5ca68e7e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,8 @@ package frc.robot; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; @@ -14,6 +16,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 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 edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; @@ -25,35 +28,24 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; +import frc.robot.subsystems.mechanisms.feeder.FeederSubsystem; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; public class RobotContainer { - // public static double MAX_SPEED = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - // public static double MAX_ANGULAR_RATE = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - // MARK: Drivetrain // Create the swerve drivetrain subsystem for the robot public final Drive drivetrain = new Drive(TunerConstants.createDrivetrain()); - - // private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final Telemetry logger = new Telemetry(DriveConstants.MAX_SPEED); - // MARK: Vision - // Uses the Quest to periodically add vision measurements - // public QuestNavSubsystem questNavSubsystem = new QuestNavSubsystem(drivetrain); - - // LimelightSubsystem limelight2Subsystem = new LimelightSubsystem(drivetrain, "limelight-two"); - // MARK: Subsystems - public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); - // public final MotorSubsystem motorSubsystem = new MotorSubsystem(); public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(drivetrain); + public final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); + public final FeederSubsystem feederSubsystem = new FeederSubsystem(); // MARK: Xbox Controllers - public final DriverJoystick driverJoystick = new DriverJoystick(new CommandXboxController(0), drivetrain, shooterSubsystem, intakeSubsystem); + public final DriverJoystick driverJoystick = new DriverJoystick(new CommandXboxController(0), drivetrain, shooterSubsystem, intakeSubsystem, feederSubsystem); public final OperatorJoystick operatorJoystick = new OperatorJoystick(new CommandXboxController(1), drivetrain, intakeSubsystem); public final DebugJoystick debugJoystick = new DebugJoystick(new CommandXboxController(2), drivetrain, shooterSubsystem, intakeSubsystem); @@ -63,16 +55,22 @@ public class RobotContainer { // MARK: Tests // public final Tests tests = new Tests(intakeSubsystem, shooterSubsystem, climberSubsystem, motorSubsystem); - // private final SendableChooser autoChooser; private final SendableChooser autoChooser; public RobotContainer() { - DriverStation.silenceJoystickConnectionWarning(true); - registerCommands.registerCommands(); - // autoChooser = new LoggedDashboardChooser<>("Autonomous Mode", AutoBuilder.buildAutoChooser()); + // MARK: Auto Chooser autoChooser = AutoBuilder.buildAutoChooser(); + autoChooser.onChange( + command -> { + Commands.runOnce( + () -> { + Logger.recordOutput("Autonomous/SelectedAuto", autoChooser.getSelected().getName()); + } + ); + } + ); SmartDashboard.putData("Auto Chooser", autoChooser); // MARK: Run Tests @@ -89,17 +87,7 @@ public RobotContainer() { private void configureBindings() { driverJoystick.configureBindings(); operatorJoystick.configureBindings(); - // debugJoystick.configureBindings(); - - // Positive X is forward, Positive Y is left according to WPILib - // drivetrain.setDefaultCommand( - // drivetrain.applyRequest(() -> - // Drive.drive - // .withVelocityX(-driverJoystick.joystick.getLeftY() * DriveConstants.MAX_SPEED) // Drive forward with negative Y (forward) - // .withVelocityY(-driverJoystick.joystick.getLeftX() * DriveConstants.MAX_SPEED) // Drive left with negative X (left) - // .withRotationalRate(-driverJoystick.joystick.getRightX() * DriveConstants.MAX_ANGULAR_RATE) // Drive counterclockwise with negative X (left) - // ) - // ); + drivetrain.setDefaultCommand(drivetrain.joysticksDefaultCommand(driverJoystick.joystick)); // Idle while the robot is disabled. This ensures the configured diff --git a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java index ca37ad2a..50bded26 100644 --- a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java +++ b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java @@ -3,6 +3,7 @@ import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; @@ -19,7 +20,7 @@ public RegisterCommands( } public void registerCommands(){ - // MARK: ShootBallsClose + // MARK: ShootFullSpeed NamedCommands.registerCommand("ShootFullSpeed", Commands.run( () -> { @@ -45,5 +46,14 @@ public void registerCommands(){ } ) ); + + // MARK: RunIntakeIn + NamedCommands.registerCommand("IntakeIn", + Commands.runOnce( + () -> { + intakeSubsystem.setIntake(IntakeStates.INTAKE_IN); + } + ) + ); } } diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index 10e0b170..26d736c4 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.mechanisms.feeder.FeederSubsystem; import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterDataPoint; @@ -15,17 +16,20 @@ public class DriverJoystick { private final Drive drivetrain; private final ShooterSubsystem shooterSubsystem; private final IntakeSubsystem intakeSubsystem; + private final FeederSubsystem feederSubsystem; public DriverJoystick( CommandXboxController joystick, Drive drivetrain, ShooterSubsystem shooterSubsystem, - IntakeSubsystem intakeSubsystem + IntakeSubsystem intakeSubsystem, + FeederSubsystem feederSubsystem ) { this.joystick = joystick; this.drivetrain = drivetrain; this.shooterSubsystem = shooterSubsystem; this.intakeSubsystem = intakeSubsystem; + this.feederSubsystem = feederSubsystem; Logger.recordOutput("DriverJoystick/ShooterSpeed", 0.0); } @@ -77,7 +81,7 @@ public void configureBindings() { // maintain motor speed shooterSubsystem.shooterMotors.drive(shooterDataPoint.speed); - shooterSubsystem.shooterFeedMotor.drive(shooterDataPoint.speed); + feederSubsystem.shooterFeederMotor.drive(shooterDataPoint.speed); shooterSubsystem.shooterPitchMotor.goTo(shooterDataPoint.angle); saveShooterDataPoint[0] = shooterDataPoint; @@ -92,7 +96,7 @@ public void configureBindings() { shooterDataPoint.speed = Math.max(0.0, shooterDataPoint.speed - 0.05); shooterSubsystem.shooterMotors.drive(shooterDataPoint.speed); - shooterSubsystem.shooterFeedMotor.drive(shooterDataPoint.speed); + feederSubsystem.shooterFeederMotor.drive(shooterDataPoint.speed); shooterSubsystem.shooterPitchMotor.goTo(0.0); diff --git a/src/main/java/frc/robot/subsystems/math/MathSubsystem.java b/src/main/java/frc/robot/subsystems/math/MathSubsystem.java index 75aad0dd..79fe6fe2 100644 --- a/src/main/java/frc/robot/subsystems/math/MathSubsystem.java +++ b/src/main/java/frc/robot/subsystems/math/MathSubsystem.java @@ -1,8 +1,6 @@ package frc.robot.subsystems.math; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - /** * MotorSubsystem provides utility methods for motion-related calculations used by * other subsystems and commands. It includes helpers to compute the signed angle @@ -14,7 +12,7 @@ * * In the future this should be moved to WhatTime */ -public class MathSubsystem extends SubsystemBase { +public class MathSubsystem { /** * Calculates the signed angular difference (in degrees) required to rotate from a * reference heading to face a target described by a 2D position vector. diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java new file mode 100644 index 00000000..2c8e8bb5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java @@ -0,0 +1,5 @@ +package frc.robot.subsystems.mechanisms.feeder; + +public class FeederConstants { + +} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java new file mode 100644 index 00000000..74ae0b41 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java @@ -0,0 +1,46 @@ +package frc.robot.subsystems.mechanisms.feeder; + +import org.littletonrobotics.junction.Logger; + +import com.btwrobotics.WhatTime.frc.MotorManagers.Motor; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class FeederSubsystem extends SubsystemBase { + // MARK: Feeder Bed + public Motor feederBedMotor = new Motor(15).setFree(true); + + // MARK: Feeder Feeder + public Motor feederFeederMotor = new Motor(16).setFree(true); + + // MARK: Shooter Feeder + public Motor shooterFeederMotor = new Motor(12).setFree(true); + + // MARK: Periodic Loop + @Override + public void periodic() { + } + + public Command runAllFeedMotors() { + return Commands.runEnd( + () -> { + feederBedMotor.drive(); + feederFeederMotor.drive(); + shooterFeederMotor.drive(); + }, + () -> { + feederBedMotor.drive(0); + feederFeederMotor.drive(0); + shooterFeederMotor.drive(0); + } + ); + } + + public void logMotorValues() { + Logger.recordOutput("FeederSubsystem/MotorConnections/FeederBedConnected", feederBedMotor.getMotor().isConnected()); + Logger.recordOutput("FeederSubsystem/MotorConnections/FeederFeederConnected", feederFeederMotor.getMotor().isConnected()); + Logger.recordOutput("FeederSubsystem/MotorConnections/ShooterFeederConnected", shooterFeederMotor.getMotor().isConnected()); + } +} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 5057f0c0..f6756b88 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -7,14 +7,11 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeSubsystem extends SubsystemBase { + // MARK: Intake Angle public Motor angleMotor = new Motor(9) .setFree(false); - @Override - public void periodic() { - Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getCurrentValue()); - } - + // MARK: Intake Wheels public Motor intakeWheelsMotor = new Motor(10) .setMinValue(IntakeConstants.minValue) .setMaxValue(IntakeConstants.maxValue) @@ -25,6 +22,12 @@ public void periodic() { .setPG(0.1) .setPositionSupplier(()-> angleMotor.getCurrentValue()); + // MARK: Periodic Loop + @Override + public void periodic() { + Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getCurrentValue()); + } + public void setPosition(double targetPosition) { Logger.recordOutput("IntakeSubsystem/SetPosition", targetPosition); intakeWheelsMotor.goTo(targetPosition); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index b883a1e2..41450479 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -66,8 +66,6 @@ public ShooterSubsystem(Drive drivetrain) { Arrays.asList(leftShooterMotor, rightShooterMotor) ).setMotorSpeed(0.4); - public Motor shooterFeedMotor = new Motor(12); - /** IMU sensor for shooter orientation feedback. */ public final Pigeon2 shooterPigeon = new Pigeon2(34); From e783ccc53e4fadbe2c8498c497d7ee450cc875e1 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Sat, 14 Mar 2026 17:21:28 -0500 Subject: [PATCH 043/102] Updated feeder subsystem to use FeederStates enum. Sets the speeds in periodic. Improved logging. Added neutral mode settings on enable and disable for brake and coast. Added speeds for feeders to FeederConstants. --- .vscode/settings.json | 3 +- README.md | 10 +++ src/main/java/frc/robot/BuildConstants.java | 10 +-- src/main/java/frc/robot/Robot.java | 38 ++++++-- src/main/java/frc/robot/RobotContainer.java | 1 - .../mechanisms/feeder/FeederConstants.java | 4 +- .../mechanisms/feeder/FeederState.java | 19 ++++ .../mechanisms/feeder/FeederSubsystem.java | 89 ++++++++++++++----- .../mechanisms/shooter/FeederState.java | 7 -- vendordeps/Phoenix6-frc2026-latest.json | 60 ++++++------- vendordeps/WhatTime.json | 4 +- 11 files changed, 170 insertions(+), 75 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederState.java delete mode 100644 src/main/java/frc/robot/subsystems/mechanisms/shooter/FeederState.java diff --git a/.vscode/settings.json b/.vscode/settings.json index c2331b7f..babf34e4 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -59,5 +59,6 @@ "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", ], - "java.dependency.enableDependencyCheckup": false + "java.dependency.enableDependencyCheckup": false, + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" } diff --git a/README.md b/README.md index a1eac699..ad882e84 100644 --- a/README.md +++ b/README.md @@ -30,3 +30,13 @@ git fetch --prune && git branch -vv | grep ': gone]' | awk '{print $1}' | xargs ![GitHub Release](https://img.shields.io/github/v/release/robohornets/WhatTime) [https://github.com/robohornets/WhatTime](https://github.com/robohornets/WhatTime) + +# QuestNav +![GitHub Release](https://img.shields.io/github/v/release/QuestNav/QuestNav) + +[https://github.com/QuestNav/QuestNav/releases](https://github.com/QuestNav/QuestNav/releases) + +# AdvantageKit +![GitHub Release](https://img.shields.io/github/v/release/Mechanical-Advantage/AdvantageKit) + +[https://github.com/Mechanical-Advantage/AdvantageKit/releases](https://github.com/Mechanical-Advantage/AdvantageKit/releases) \ No newline at end of file diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 17ccac0c..16de96b0 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026 Roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 165; - public static final String GIT_SHA = "8467f22af087c6a5a9451783d9b3d230186a6022"; - public static final String GIT_DATE = "2026-03-07 15:13:35 CST"; + public static final int GIT_REVISION = 167; + public static final String GIT_SHA = "ced371b7da68c983389924811c84fac52cdffcea"; + public static final String GIT_DATE = "2026-03-11 17:38:02 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-09 17:42:38 CDT"; - public static final long BUILD_UNIX_TIME = 1773096158530L; + public static final String BUILD_DATE = "2026-03-14 02:23:48 CDT"; + public static final long BUILD_UNIX_TIME = 1773473028445L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4a2f6956..6e8c724a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -91,17 +91,12 @@ public void robotInit() { DriverStation.silenceJoystickConnectionWarning(true); - currentAlliance = DriverStation.getAlliance(); - robotContainer.shooterSubsystem.shooterPitchMotor.setNeutralMode(NeutralModeValue.Brake); robotContainer.intakeSubsystem.angleMotor.setNeutralMode(NeutralModeValue.Brake); // Reset motor speeds to zero. robotContainer.intakeSubsystem.angleMotor.set(0.0); robotContainer.shooterSubsystem.shooterPitchMotor.set(0.0); - - - Logger.recordOutput("FieldInfo/CurrentAlliance", currentAlliance.toString()); } // MARK: Robot Periodic @@ -114,11 +109,26 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); logDriveStationValues(); + + if (!currentAlliance.equals(DriverStation.getAlliance())) { + currentAlliance = DriverStation.getAlliance(); + Logger.recordOutput("FieldInfo/CurrentAlliance", currentAlliance.toString()); + } } // MARK: Disabled Init @Override - public void disabledInit() {} + public void disabledInit() { + NeutralModeValue settingNeutralModeValue = NeutralModeValue.Coast; + robotContainer.feederSubsystem.feederBedMotor.setNeutralMode(settingNeutralModeValue); + robotContainer.feederSubsystem.feederFeederMotor.setNeutralMode(settingNeutralModeValue); + robotContainer.feederSubsystem.shooterFeederMotor.setNeutralMode(settingNeutralModeValue); + + robotContainer.shooterSubsystem.shooterPitchMotor.setNeutralMode(settingNeutralModeValue); + + robotContainer.intakeSubsystem.angleMotor.setNeutralMode(settingNeutralModeValue); + robotContainer.intakeSubsystem.intakeWheelsMotor.setNeutralMode(settingNeutralModeValue); + } // MARK: Disabled Periodic @Override @@ -126,7 +136,20 @@ public void disabledPeriodic() {} // MARK: Disabled Exit @Override - public void disabledExit() {} + public void disabledExit() { + NeutralModeValue settingNeutralModeValue = NeutralModeValue.Brake; + robotContainer.feederSubsystem.feederBedMotor.setNeutralMode(settingNeutralModeValue); + robotContainer.feederSubsystem.feederFeederMotor.setNeutralMode(settingNeutralModeValue); + robotContainer.feederSubsystem.shooterFeederMotor.setNeutralMode(settingNeutralModeValue); + + robotContainer.shooterSubsystem.shooterPitchMotor.setNeutralMode(settingNeutralModeValue); + + robotContainer.intakeSubsystem.angleMotor.setNeutralMode(settingNeutralModeValue); + robotContainer.intakeSubsystem.intakeWheelsMotor.setNeutralMode(settingNeutralModeValue); + + robotContainer.shooterSubsystem.leftShooterMotor.setNeutralMode(NeutralModeValue.Coast); + robotContainer.shooterSubsystem.rightShooterMotor.setNeutralMode(NeutralModeValue.Coast); + } // MARK: Autonomous Init @Override @@ -186,6 +209,7 @@ public void testExit() {} public void simulationPeriodic() {} + // MARK: Log DriverStation private void logDriveStationValues() { Logger.recordOutput("DriverStation/GameSpecificMessage", DriverStation.getGameSpecificMessage()); Logger.recordOutput("DriverStation/MatchTime", DriverStation.getMatchTime()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5ca68e7e..a6b44913 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,7 +12,6 @@ import com.pathplanner.lib.commands.FollowPathCommand; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java index 2c8e8bb5..5067ced9 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.mechanisms.feeder; public class FeederConstants { - + public static double FEEDER_BED_SPEED = 0.5; + public static double FEEDER_FEEDER_SPEED = 0.5; + public static double SHOOTER_FEEDER_SPEED = 0.5; } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederState.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederState.java new file mode 100644 index 00000000..62da4189 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederState.java @@ -0,0 +1,19 @@ +package frc.robot.subsystems.mechanisms.feeder; + + +public enum FeederState { + /** Runs all motors in the feeder subsystem in towards the shooter. */ + ALL_FEEDER_IN, + /** Runs all motors in the feeder subsystem out away from the shooter. */ + ALL_FEEDER_OUT, + /** Runs only the bed rollers towards the shooter. */ + ROLLERS_IN, + /** Runs only the bed rollers away from the shooter. */ + ROLLERS_OUT, + /** Runs the shooter feed and feeder feeder motors to control feeding to the shooter. */ + SHOOTER_FEED_IN, + /** Runs the shooter feed and feeder feeder motors out to remove balls from the shooter in the event they get stuck. */ + SHOOTER_FEED_OUT, + /** Stops all motors in the feeder subsystem. */ + OFF; +} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java index 74ae0b41..64780a6d 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java @@ -1,46 +1,93 @@ package frc.robot.subsystems.mechanisms.feeder; import org.littletonrobotics.junction.Logger; - import com.btwrobotics.WhatTime.frc.MotorManagers.Motor; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; + public class FeederSubsystem extends SubsystemBase { // MARK: Feeder Bed - public Motor feederBedMotor = new Motor(15).setFree(true); + public Motor feederBedMotor = new Motor(15).setFree(true).setMotorSpeed(FeederConstants.FEEDER_BED_SPEED); // MARK: Feeder Feeder - public Motor feederFeederMotor = new Motor(16).setFree(true); + public Motor feederFeederMotor = new Motor(16).setFree(true).setMotorSpeed(FeederConstants.FEEDER_FEEDER_SPEED); // MARK: Shooter Feeder - public Motor shooterFeederMotor = new Motor(12).setFree(true); + public Motor shooterFeederMotor = new Motor(12).setFree(true).setMotorSpeed(FeederConstants.SHOOTER_FEEDER_SPEED); + + // MARK: Feeder State + private FeederState feederState = FeederState.OFF; // MARK: Periodic Loop @Override public void periodic() { + switch (feederState) { + case ALL_FEEDER_IN: + runSpecifiedMotors(true, false, true, false, true, false); + break; + + case ALL_FEEDER_OUT: + runSpecifiedMotors(true, true, true, true, true, true); + break; + + case ROLLERS_IN: + runSpecifiedMotors(true, false, false, false, false, false); + break; + + case ROLLERS_OUT: + runSpecifiedMotors(true, true, false, false, false, false); + break; + + case SHOOTER_FEED_IN: + runSpecifiedMotors(false, false, true, false, true, false); + break; + + case SHOOTER_FEED_OUT: + runSpecifiedMotors(false, false, true, true, true, true); + break; + + case OFF: + runSpecifiedMotors(false, false, false, false, false, false); + break; + + default: + runSpecifiedMotors(false, false, false, false, false, false); + break; + } + } + + private void runSpecifiedMotors( + boolean runFeederBed, + boolean feederBedInverted, + boolean runFeederFeeder, + boolean feederFeederInverted, + boolean runShooterFeeder, + boolean shooterFeederInverted + ) { + feederBedMotor.toggleEnabled(runFeederBed); + feederFeederMotor.toggleEnabled(runFeederFeeder); + shooterFeederMotor.toggleEnabled(runShooterFeeder); + + if (runFeederBed) { + feederBedMotor.drive(feederBedInverted); + } + if (runFeederFeeder) { + feederFeederMotor.drive(feederFeederInverted); + } + if (runShooterFeeder) { + shooterFeederMotor.drive(shooterFeederInverted); + } } - public Command runAllFeedMotors() { - return Commands.runEnd( - () -> { - feederBedMotor.drive(); - feederFeederMotor.drive(); - shooterFeederMotor.drive(); - }, - () -> { - feederBedMotor.drive(0); - feederFeederMotor.drive(0); - shooterFeederMotor.drive(0); - } - ); + public void setFeederState(FeederState feederState) { + this.feederState = feederState; } - public void logMotorValues() { + public void logValues() { Logger.recordOutput("FeederSubsystem/MotorConnections/FeederBedConnected", feederBedMotor.getMotor().isConnected()); Logger.recordOutput("FeederSubsystem/MotorConnections/FeederFeederConnected", feederFeederMotor.getMotor().isConnected()); Logger.recordOutput("FeederSubsystem/MotorConnections/ShooterFeederConnected", shooterFeederMotor.getMotor().isConnected()); + + Logger.recordOutput("FeederSubsystem/FeederState", feederState.toString()); } } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/FeederState.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/FeederState.java deleted file mode 100644 index e149a79f..00000000 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/FeederState.java +++ /dev/null @@ -1,7 +0,0 @@ -package frc.robot.subsystems.mechanisms.shooter; - -public enum FeederState { - FEEDER_IN, - FEEDER_OUT, - OFF; -} diff --git a/vendordeps/Phoenix6-frc2026-latest.json b/vendordeps/Phoenix6-frc2026-latest.json index d6cf160b..97ea239c 100644 --- a/vendordeps/Phoenix6-frc2026-latest.json +++ b/vendordeps/Phoenix6-frc2026-latest.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6-frc2026-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.1", + "version": "26.1.2", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.1" + "version": "26.1.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.1", + "version": "26.1.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.1", + "version": "26.1.2", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/WhatTime.json b/vendordeps/WhatTime.json index 7911b7d1..5b349c6f 100644 --- a/vendordeps/WhatTime.json +++ b/vendordeps/WhatTime.json @@ -1,7 +1,7 @@ { "fileName": "WhatTime.json", "name": "WhatTime", - "version": "v2026.2.1-beta.1", + "version": "v2026.2.1-beta.2", "frcYear": 2026, "uuid": "2C0B26E9-47AA-4C55-B235-640C45ECAD33", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.btwrobotics.whattime.frc", "artifactId": "whattime", - "version": "v2026.2.1-beta.1" + "version": "v2026.2.1-beta.2" } ], "jniDependencies": [], From e508b2e38c13d9f77e15231617a090794dc244f7 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Mon, 16 Mar 2026 17:59:18 -0500 Subject: [PATCH 044/102] Minor changes to add feeder subsystem to joysticks and stuff. --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../commands/namedcommands/RegisterCommands.java | 16 +++++++++++++++- .../java/frc/robot/joysticks/DebugJoystick.java | 11 +++++++++-- 3 files changed, 26 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a6b44913..c6286995 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -46,10 +46,10 @@ public class RobotContainer { // MARK: Xbox Controllers public final DriverJoystick driverJoystick = new DriverJoystick(new CommandXboxController(0), drivetrain, shooterSubsystem, intakeSubsystem, feederSubsystem); public final OperatorJoystick operatorJoystick = new OperatorJoystick(new CommandXboxController(1), drivetrain, intakeSubsystem); - public final DebugJoystick debugJoystick = new DebugJoystick(new CommandXboxController(2), drivetrain, shooterSubsystem, intakeSubsystem); + public final DebugJoystick debugJoystick = new DebugJoystick(new CommandXboxController(2), drivetrain, shooterSubsystem, intakeSubsystem, feederSubsystem); // MARK: Register Commands - public final RegisterCommands registerCommands = new RegisterCommands(intakeSubsystem, shooterSubsystem); + public final RegisterCommands registerCommands = new RegisterCommands(intakeSubsystem, shooterSubsystem, feederSubsystem); // MARK: Tests // public final Tests tests = new Tests(intakeSubsystem, shooterSubsystem, climberSubsystem, motorSubsystem); diff --git a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java index 50bded26..b866ac10 100644 --- a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java +++ b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java @@ -3,6 +3,8 @@ import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.mechanisms.feeder.FeederState; +import frc.robot.subsystems.mechanisms.feeder.FeederSubsystem; import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; @@ -10,13 +12,16 @@ public class RegisterCommands { IntakeSubsystem intakeSubsystem; ShooterSubsystem shooterSubsystem; + FeederSubsystem feederSubsystem; public RegisterCommands( IntakeSubsystem intakeSubsystem, - ShooterSubsystem shooterSubsystem + ShooterSubsystem shooterSubsystem, + FeederSubsystem feederSubsystem ) { this.intakeSubsystem = intakeSubsystem; this.shooterSubsystem = shooterSubsystem; + this.feederSubsystem = feederSubsystem; } public void registerCommands(){ @@ -55,5 +60,14 @@ public void registerCommands(){ } ) ); + + // MARK: + NamedCommands.registerCommand("RunAllFeederIn", + Commands.runOnce( + () -> { + feederSubsystem.setFeederState(FeederState.ALL_FEEDER_IN); + } + ) + ); } } diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index e0dea5df..8eae205a 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -9,6 +9,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.mechanisms.feeder.FeederState; +import frc.robot.subsystems.mechanisms.feeder.FeederSubsystem; import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; @@ -20,6 +22,7 @@ public class DebugJoystick { private final Drive drivetrain; private final ShooterSubsystem shooterSubsystem; private final IntakeSubsystem intakeSubsystem; + private final FeederSubsystem feederSubsystem; private double shooterSpeed; private double shooterPitch; @@ -28,12 +31,14 @@ public DebugJoystick( CommandXboxController joystick, Drive drivetrain, ShooterSubsystem shooterSubsystem, - IntakeSubsystem intakeSubsystem + IntakeSubsystem intakeSubsystem, + FeederSubsystem feederSubsystem ) { this.joystick = joystick; this.drivetrain = drivetrain; this.shooterSubsystem = shooterSubsystem; this.intakeSubsystem = intakeSubsystem; + this.feederSubsystem = feederSubsystem; this.shooterSpeed = 0.0; this.shooterPitch = 0.0; @@ -81,7 +86,9 @@ public void configureBindings() { NamedCommands.getCommand("IntakeUp") ); - joystick.x(); + joystick.x().whileTrue( + NamedCommands.getCommand("RunAllFeederIn") + ); joystick.y(); From 88d3e96dbb771c89a0cb8b2f00091a493963aa93 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Mon, 16 Mar 2026 18:01:32 -0500 Subject: [PATCH 045/102] More joystick adjustments --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/joysticks/OperatorJoystick.java | 12 ++++++++++-- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c6286995..02213438 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -45,7 +45,7 @@ public class RobotContainer { // MARK: Xbox Controllers public final DriverJoystick driverJoystick = new DriverJoystick(new CommandXboxController(0), drivetrain, shooterSubsystem, intakeSubsystem, feederSubsystem); - public final OperatorJoystick operatorJoystick = new OperatorJoystick(new CommandXboxController(1), drivetrain, intakeSubsystem); + public final OperatorJoystick operatorJoystick = new OperatorJoystick(new CommandXboxController(1), drivetrain, shooterSubsystem, intakeSubsystem, feederSubsystem); public final DebugJoystick debugJoystick = new DebugJoystick(new CommandXboxController(2), drivetrain, shooterSubsystem, intakeSubsystem, feederSubsystem); // MARK: Register Commands diff --git a/src/main/java/frc/robot/joysticks/OperatorJoystick.java b/src/main/java/frc/robot/joysticks/OperatorJoystick.java index b1c2eb14..d0125ea4 100644 --- a/src/main/java/frc/robot/joysticks/OperatorJoystick.java +++ b/src/main/java/frc/robot/joysticks/OperatorJoystick.java @@ -4,21 +4,29 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.mechanisms.feeder.FeederSubsystem; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; +import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; public class OperatorJoystick { public final CommandXboxController joystick; private final Drive drivetrain; + private final ShooterSubsystem shooterSubsystem; private final IntakeSubsystem intakeSubsystem; + private final FeederSubsystem feederSubsystem; public OperatorJoystick( CommandXboxController joystick, - Drive drivetrain, - IntakeSubsystem intakeSubsystem + Drive drivetrain, + ShooterSubsystem shooterSubsystem, + IntakeSubsystem intakeSubsystem, + FeederSubsystem feederSubsystem ) { this.joystick = joystick; this.drivetrain = drivetrain; + this.shooterSubsystem = shooterSubsystem; this.intakeSubsystem = intakeSubsystem; + this.feederSubsystem = feederSubsystem; } From e67db5789d1f29a67b2ec44d2d33234ae986d2ba Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Mon, 16 Mar 2026 23:41:29 -0500 Subject: [PATCH 046/102] Added current limits to more motors. Added current logging to NetworkTables. --- src/main/java/frc/robot/RobotContainer.java | 15 ++++++++ .../frc/robot/generated/TunerConstants.java | 15 ++++++-- .../frc/robot/subsystems/drive/Drive.java | 15 ++++++++ .../mechanisms/feeder/FeederSubsystem.java | 18 ++++++++-- .../mechanisms/intake/IntakeSubsystem.java | 10 ++++-- .../mechanisms/shooter/ShooterSubsystem.java | 35 +++++++++++-------- 6 files changed, 86 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 02213438..9fbdc7a6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,8 +4,12 @@ package frc.robot; +import static edu.wpi.first.units.Units.Amps; + import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; @@ -50,6 +54,16 @@ public class RobotContainer { // MARK: Register Commands public final RegisterCommands registerCommands = new RegisterCommands(intakeSubsystem, shooterSubsystem, feederSubsystem); + + // MARK: Motor Config + public static final TalonFXConfiguration mechanismsMotorConfiguration = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Amps.of(60)) + .withSupplyCurrentLimit(Amps.of(40)) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimitEnable(true) + ); // MARK: Tests // public final Tests tests = new Tests(intakeSubsystem, shooterSubsystem, climberSubsystem, motorSubsystem); @@ -110,6 +124,7 @@ private void configureBindings() { drivetrain.registerTelemetry(logger::telemeterize); } + // MARK: Get Autonomous public Command getAutonomousCommand() { /* Run the path selected from the auto chooser */ // return autoChooser.get(); diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index b9adaa65..06e17620 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -55,14 +55,25 @@ public class TunerConstants { // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(80)) + .withSupplyCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimitEnable(true) + ); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() // Swerve azimuth does not require much torque output, so we can set a relatively low // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimit(Amps.of(80)) + .withSupplyCurrentLimit(Amps.of(60)) .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimitEnable(true) ); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 15d5f635..f8cabe3b 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -275,4 +275,19 @@ public static Pose2d flipAlliance(Pose2d pose) { } return pose; } + + // MARK: Motor Logging + public void logMotorInformation() { + Logger.recordOutput("SwerveDrive/Motors/Current/DriveMotor0", drivetrain.getModule(0).getSteerMotor().getStatorCurrent().getValueAsDouble()); + Logger.recordOutput("SwerveDrive/Motors/Current/SteerMotor0", drivetrain.getModule(0).getDriveMotor().getStatorCurrent().getValueAsDouble()); + + Logger.recordOutput("SwerveDrive/Motors/Current/DriveMotor1", drivetrain.getModule(1).getSteerMotor().getStatorCurrent().getValueAsDouble()); + Logger.recordOutput("SwerveDrive/Motors/Current/SteerMotor1", drivetrain.getModule(1).getDriveMotor().getStatorCurrent().getValueAsDouble()); + + Logger.recordOutput("SwerveDrive/Motors/Current/DriveMotor2", drivetrain.getModule(2).getSteerMotor().getStatorCurrent().getValueAsDouble()); + Logger.recordOutput("SwerveDrive/Motors/Current/SteerMotor2", drivetrain.getModule(2).getDriveMotor().getStatorCurrent().getValueAsDouble()); + + Logger.recordOutput("SwerveDrive/Motors/Current/DriveMotor3", drivetrain.getModule(3).getSteerMotor().getStatorCurrent().getValueAsDouble()); + Logger.recordOutput("SwerveDrive/Motors/Current/SteerMotor3", drivetrain.getModule(3).getDriveMotor().getStatorCurrent().getValueAsDouble()); + } } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java index 64780a6d..e67d759b 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java @@ -2,22 +2,34 @@ import org.littletonrobotics.junction.Logger; import com.btwrobotics.WhatTime.frc.MotorManagers.Motor; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; + import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotContainer; public class FeederSubsystem extends SubsystemBase { // MARK: Feeder Bed - public Motor feederBedMotor = new Motor(15).setFree(true).setMotorSpeed(FeederConstants.FEEDER_BED_SPEED); + public Motor feederBedMotor = new Motor(15, "Mechanisms").setFree(true).setMotorSpeed(FeederConstants.FEEDER_BED_SPEED); // MARK: Feeder Feeder - public Motor feederFeederMotor = new Motor(16).setFree(true).setMotorSpeed(FeederConstants.FEEDER_FEEDER_SPEED); + public Motor feederFeederMotor = new Motor(16, "Mechanisms").setFree(true).setMotorSpeed(FeederConstants.FEEDER_FEEDER_SPEED); // MARK: Shooter Feeder - public Motor shooterFeederMotor = new Motor(12).setFree(true).setMotorSpeed(FeederConstants.SHOOTER_FEEDER_SPEED); + public Motor shooterFeederMotor = new Motor(12, "Mechanisms").setFree(true).setMotorSpeed(FeederConstants.SHOOTER_FEEDER_SPEED); // MARK: Feeder State private FeederState feederState = FeederState.OFF; + // Applys current limits to motors to reduce chance of brownout + // MARK: Constructor + public FeederSubsystem() { + feederBedMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + feederFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + shooterFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + } + // MARK: Periodic Loop @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index f6756b88..8c3ab16b 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -5,14 +5,15 @@ import com.btwrobotics.WhatTime.frc.MotorManagers.Motor; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotContainer; public class IntakeSubsystem extends SubsystemBase { // MARK: Intake Angle - public Motor angleMotor = new Motor(9) + public Motor angleMotor = new Motor(9, "Mechanisms") .setFree(false); // MARK: Intake Wheels - public Motor intakeWheelsMotor = new Motor(10) + public Motor intakeWheelsMotor = new Motor(10, "Mechanisms") .setMinValue(IntakeConstants.minValue) .setMaxValue(IntakeConstants.maxValue) .setMotorSpeed(0.2) @@ -22,6 +23,11 @@ public class IntakeSubsystem extends SubsystemBase { .setPG(0.1) .setPositionSupplier(()-> angleMotor.getCurrentValue()); + public IntakeSubsystem() { + angleMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + intakeWheelsMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + } + // MARK: Periodic Loop @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 41450479..bebc8288 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -12,6 +12,7 @@ import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotContainer; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.math.MathSubsystem; @@ -38,6 +39,21 @@ public class ShooterSubsystem extends SubsystemBase { public ShooterConstants shooterConstants = new ShooterConstants(); + // MARK: Motors + /** Motor controlling the shooter pitch (angle). */ + // this is reversed in real life so its inverted in the code + public final Motor shooterPitchMotor = new Motor(11, "Mechanisms", true) + .setFree(false) + .setRange(-0.5, 0) + .setMotorSpeed(0.1); + + public final Motor leftShooterMotor = new Motor(13, "Mechanisms"); + public final Motor rightShooterMotor = new Motor(14, "Mechanisms", true); + + public final MotorGroup shooterMotors = new MotorGroup( + Arrays.asList(leftShooterMotor, rightShooterMotor) + ).setMotorSpeed(0.4); + // MARK: Constructor /** * Constructs the ShooterSubsystem. @@ -45,26 +61,15 @@ public class ShooterSubsystem extends SubsystemBase { */ public ShooterSubsystem(Drive drivetrain) { this.drivetrain = drivetrain; + + shooterPitchMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + leftShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + rightShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); for (ShooterDataPoint point : ShooterConstants.shooterDataPoints) { dataPoints.put(point.distance, point); } } - - /** Motor controlling the shooter pitch (angle). */ - - // this is reversed in real life so its inverted in the code - public final Motor shooterPitchMotor = new Motor(11, true) - .setFree(false) - .setRange(-0.5, 0) - .setMotorSpeed(0.1); - - public final Motor leftShooterMotor = new Motor(13); - public final Motor rightShooterMotor = new Motor(14, true); - - public final MotorGroup shooterMotors = new MotorGroup( - Arrays.asList(leftShooterMotor, rightShooterMotor) - ).setMotorSpeed(0.4); /** IMU sensor for shooter orientation feedback. */ public final Pigeon2 shooterPigeon = new Pigeon2(34); From e822ff25fc2605c0d253fbcad5dd1d725919ddd9 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 10:50:37 -0500 Subject: [PATCH 047/102] Added better logging for motor current/voltage for swerve modules. --- .../frc/robot/joysticks/DebugJoystick.java | 1 - .../frc/robot/subsystems/drive/Drive.java | 73 ++++++++++++------- .../subsystems/drive/DriveConstants.java | 2 + .../mechanisms/feeder/FeederSubsystem.java | 2 - 4 files changed, 48 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 8eae205a..c3edc1cb 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.mechanisms.feeder.FeederState; import frc.robot.subsystems.mechanisms.feeder.FeederSubsystem; import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index f8cabe3b..81a1c6a7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -1,9 +1,5 @@ package frc.robot.subsystems.drive; -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.function.Consumer; import java.util.function.Supplier; @@ -32,7 +28,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.vision.limelight.LimelightSubsystem; import frc.robot.subsystems.vision.questnav.QuestNavSubsystem; @@ -42,41 +37,37 @@ public class Drive extends SubsystemBase { public QuestNavSubsystem questNavSubsystem; - LimelightSubsystem limelightSubsystem; + public LimelightSubsystem limelightSubsystem; + // MARK: Constructor public Drive(CommandSwerveDrivetrain drivetrain) { this.drivetrain = drivetrain; this.questNavSubsystem = new QuestNavSubsystem(this); - this.limelightSubsystem = new LimelightSubsystem(this, questNavSubsystem, "limelight-four"); configureAutoBuilder(); } - public static final double ODOMETRY_FREQUENCY = 250.0; - public static double MaxSpeed = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - public static double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - // MARK: Field Centric public static final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1) - .withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .withDeadband(DriveConstants.MAX_SPEED * 0.1) + .withRotationalDeadband(DriveConstants.MAX_ANGULAR_RATE * 0.1) // Add a 10% deadband .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors // MARK: Heading Control public final SwerveRequest.FieldCentricFacingAngle driveFacingHub = new SwerveRequest.FieldCentricFacingAngle() .withHeadingPID(5, 0, 0) - .withDeadband(MaxSpeed * 0.1) - .withRotationalDeadband(MaxAngularRate * 0.1) + .withDeadband(DriveConstants.MAX_SPEED * 0.1) + .withRotationalDeadband(DriveConstants.MAX_ANGULAR_RATE * 0.1) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // MARK: Robot Centric public static final SwerveRequest.RobotCentric driveRobotCentric = new SwerveRequest.RobotCentric() - .withDeadband(MaxSpeed * 0.1) - .withRotationalDeadband(MaxAngularRate * 0.1) + .withDeadband(DriveConstants.MAX_SPEED * 0.1) + .withRotationalDeadband(DriveConstants.MAX_ANGULAR_RATE * 0.1) .withDriveRequestType(DriveRequestType.OpenLoopVoltage); public final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); @@ -98,6 +89,8 @@ public void periodic() { Logger.recordOutput("SwerveDrive/Rotation", getPose2d().getRotation()); Logger.recordOutput("SwerveDrive/TargetHubAngle", getAngleToHub()); + + logMotorInformation(); } public Command applyRequest(Supplier request) { @@ -278,16 +271,42 @@ public static Pose2d flipAlliance(Pose2d pose) { // MARK: Motor Logging public void logMotorInformation() { - Logger.recordOutput("SwerveDrive/Motors/Current/DriveMotor0", drivetrain.getModule(0).getSteerMotor().getStatorCurrent().getValueAsDouble()); - Logger.recordOutput("SwerveDrive/Motors/Current/SteerMotor0", drivetrain.getModule(0).getDriveMotor().getStatorCurrent().getValueAsDouble()); + for (int i = 0; i <= 3; i++) { + // Logs the current readings to AdvantageKit + Logger.recordOutput( + "SwerveDrive/Motors/Current/Stator/SteerMotor" + i, + drivetrain.getModule(i).getSteerMotor().getStatorCurrent().getValueAsDouble() + ); + Logger.recordOutput( + "SwerveDrive/Motors/Current/Stator/DriveMotor" + i, + drivetrain.getModule(i).getDriveMotor().getStatorCurrent().getValueAsDouble() + ); + Logger.recordOutput( + "SwerveDrive/Motors/Current/Supply/SteerMotor" + i, + drivetrain.getModule(i).getSteerMotor().getSupplyCurrent().getValueAsDouble() + ); + Logger.recordOutput( + "SwerveDrive/Motors/Current/Supply/DriveMotor" + i, + drivetrain.getModule(i).getDriveMotor().getSupplyCurrent().getValueAsDouble() + ); - Logger.recordOutput("SwerveDrive/Motors/Current/DriveMotor1", drivetrain.getModule(1).getSteerMotor().getStatorCurrent().getValueAsDouble()); - Logger.recordOutput("SwerveDrive/Motors/Current/SteerMotor1", drivetrain.getModule(1).getDriveMotor().getStatorCurrent().getValueAsDouble()); - - Logger.recordOutput("SwerveDrive/Motors/Current/DriveMotor2", drivetrain.getModule(2).getSteerMotor().getStatorCurrent().getValueAsDouble()); - Logger.recordOutput("SwerveDrive/Motors/Current/SteerMotor2", drivetrain.getModule(2).getDriveMotor().getStatorCurrent().getValueAsDouble()); - - Logger.recordOutput("SwerveDrive/Motors/Current/DriveMotor3", drivetrain.getModule(3).getSteerMotor().getStatorCurrent().getValueAsDouble()); - Logger.recordOutput("SwerveDrive/Motors/Current/SteerMotor3", drivetrain.getModule(3).getDriveMotor().getStatorCurrent().getValueAsDouble()); + // Logs the voltage to AdvantageKit + Logger.recordOutput( + "SwerveDrive/Motors/Voltage/Output/SteerMotor" + i, + drivetrain.getModule(i).getDriveMotor().getMotorVoltage().getValueAsDouble() + ); + Logger.recordOutput( + "SwerveDrive/Motors/Voltage/Output/DriveMotor" + i, + drivetrain.getModule(i).getDriveMotor().getMotorVoltage().getValueAsDouble() + ); + Logger.recordOutput( + "SwerveDrive/Motors/Voltage/Supply/SteerMotor" + i, + drivetrain.getModule(i).getDriveMotor().getSupplyVoltage().getValueAsDouble() + ); + Logger.recordOutput( + "SwerveDrive/Motors/Voltage/Supply/DriveMotor" + i, + drivetrain.getModule(i).getDriveMotor().getStatorCurrent().getValueAsDouble() + ); + } } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 49908538..236c1b7c 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -14,6 +14,8 @@ public class DriveConstants { // 3/4 of a rotation per second max angular velocity public static double MAX_ANGULAR_RATE = RotationsPerSecond.of(0.75).in(RadiansPerSecond); + public static final double ODOMETRY_FREQUENCY = 250.0; + // MARK: Coordinates of Hub public static Translation2d HUB_RED_POSITION = new Translation2d(11.915, 4.034); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java index e67d759b..d9196ab0 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java @@ -2,8 +2,6 @@ import org.littletonrobotics.junction.Logger; import com.btwrobotics.WhatTime.frc.MotorManagers.Motor; -import com.ctre.phoenix6.configs.CurrentLimitsConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotContainer; From 92059eb0f6fb94850e63c7945c1114c51f68742c Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 11:58:41 -0500 Subject: [PATCH 048/102] Added code to debug joystick for testing. Added automatic bed agitation routine to feeder subsystem. Updated Limelight Rotation2d. --- src/main/java/frc/robot/BuildConstants.java | 10 ++-- .../frc/robot/joysticks/DebugJoystick.java | 19 ++++++-- .../mechanisms/feeder/FeederState.java | 4 -- .../mechanisms/feeder/FeederSubsystem.java | 48 +++++++++++-------- .../mechanisms/shooter/ShooterConstants.java | 34 ++++++++----- .../mechanisms/shooter/ShooterSubsystem.java | 12 ++++- .../vision/limelight/LimelightConstants.java | 4 +- 7 files changed, 86 insertions(+), 45 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 16de96b0..3a870fdf 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026 Roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 167; - public static final String GIT_SHA = "ced371b7da68c983389924811c84fac52cdffcea"; - public static final String GIT_DATE = "2026-03-11 17:38:02 CDT"; + public static final int GIT_REVISION = 172; + public static final String GIT_SHA = "e822ff25fc2605c0d253fbcad5dd1d725919ddd9"; + public static final String GIT_DATE = "2026-03-17 10:50:37 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-14 02:23:48 CDT"; - public static final long BUILD_UNIX_TIME = 1773473028445L; + public static final String BUILD_DATE = "2026-03-17 11:56:58 CDT"; + public static final long BUILD_UNIX_TIME = 1773766618484L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index c3edc1cb..710070c2 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; @@ -86,12 +87,24 @@ public void configureBindings() { ); joystick.x().whileTrue( - NamedCommands.getCommand("RunAllFeederIn") + Commands.run( + () -> { + shooterSubsystem.incrementShooterAngle(0.01); + } + ) ); - joystick.y(); + joystick.y().whileTrue( + Commands.run( + () -> { + shooterSubsystem.incrementShooterAngle(-0.01); + } + ) + );; - joystick.rightTrigger(); + joystick.rightTrigger().whileTrue( + NamedCommands.getCommand("RunAllFeederIn") + ); joystick.leftTrigger() .whileTrue( diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederState.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederState.java index 62da4189..8a65897e 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederState.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederState.java @@ -6,10 +6,6 @@ public enum FeederState { ALL_FEEDER_IN, /** Runs all motors in the feeder subsystem out away from the shooter. */ ALL_FEEDER_OUT, - /** Runs only the bed rollers towards the shooter. */ - ROLLERS_IN, - /** Runs only the bed rollers away from the shooter. */ - ROLLERS_OUT, /** Runs the shooter feed and feeder feeder motors to control feeding to the shooter. */ SHOOTER_FEED_IN, /** Runs the shooter feed and feeder feeder motors out to remove balls from the shooter in the event they get stuck. */ diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java index d9196ab0..632dcacc 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java @@ -3,6 +3,8 @@ import org.littletonrobotics.junction.Logger; import com.btwrobotics.WhatTime.frc.MotorManagers.Motor; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotContainer; @@ -26,6 +28,8 @@ public FeederSubsystem() { feederBedMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); feederFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); shooterFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + + motorBedAgitationRoutine(); } // MARK: Periodic Loop @@ -33,54 +37,57 @@ public FeederSubsystem() { public void periodic() { switch (feederState) { case ALL_FEEDER_IN: - runSpecifiedMotors(true, false, true, false, true, false); + runSpecifiedMotors(true, false, true, false); break; case ALL_FEEDER_OUT: - runSpecifiedMotors(true, true, true, true, true, true); - break; - - case ROLLERS_IN: - runSpecifiedMotors(true, false, false, false, false, false); - break; - - case ROLLERS_OUT: - runSpecifiedMotors(true, true, false, false, false, false); + runSpecifiedMotors(true, true, true, true); break; case SHOOTER_FEED_IN: - runSpecifiedMotors(false, false, true, false, true, false); + runSpecifiedMotors(true, false, true, false); break; case SHOOTER_FEED_OUT: - runSpecifiedMotors(false, false, true, true, true, true); + runSpecifiedMotors(true, true, true, true); break; case OFF: - runSpecifiedMotors(false, false, false, false, false, false); + runSpecifiedMotors(false, false, false, false); break; default: - runSpecifiedMotors(false, false, false, false, false, false); + runSpecifiedMotors(false, false, false, false); break; } } + // MARK: Bed Agitation + // Reverses the bed roller motors every 10 seconds + private Command motorBedAgitationRoutine() { + return Commands.repeatingSequence( + Commands.run( + () -> { + feederBedMotor.drive(); + } + ).withTimeout(10), + Commands.run( + () -> { + feederBedMotor.drive(true); + } + ).withTimeout(2) + ); + } + private void runSpecifiedMotors( - boolean runFeederBed, - boolean feederBedInverted, boolean runFeederFeeder, boolean feederFeederInverted, boolean runShooterFeeder, boolean shooterFeederInverted ) { - feederBedMotor.toggleEnabled(runFeederBed); feederFeederMotor.toggleEnabled(runFeederFeeder); shooterFeederMotor.toggleEnabled(runShooterFeeder); - if (runFeederBed) { - feederBedMotor.drive(feederBedInverted); - } if (runFeederFeeder) { feederFeederMotor.drive(feederFeederInverted); } @@ -93,6 +100,7 @@ public void setFeederState(FeederState feederState) { this.feederState = feederState; } + // MARK: Logging public void logValues() { Logger.recordOutput("FeederSubsystem/MotorConnections/FeederBedConnected", feederBedMotor.getMotor().isConnected()); Logger.recordOutput("FeederSubsystem/MotorConnections/FeederFeederConnected", feederFeederMotor.getMotor().isConnected()); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java index 67198a97..9862ce94 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java @@ -3,9 +3,21 @@ import java.util.List; public class ShooterConstants { - public static List shooterDataPoints = List.of( + public static final List shooterDataPoints = List.of( // TODO: Collect successful data points and add them here - new ShooterDataPoint(0, 0, 0) + new ShooterDataPoint(0.0, 0, 0), + new ShooterDataPoint(0.5, 0, 0), + new ShooterDataPoint(1.0, 0, 0), + new ShooterDataPoint(1.5, 0, 0), + new ShooterDataPoint(2.0, 0, 0), + new ShooterDataPoint(2.5, 0, 0), + new ShooterDataPoint(3.0, 0, 0), + new ShooterDataPoint(3.5, 0, 0), + new ShooterDataPoint(4.0, 0, 0), + new ShooterDataPoint(4.5, 0, 0), + new ShooterDataPoint(5.0, 0, 0), + new ShooterDataPoint(5.5, 0, 0), + new ShooterDataPoint(6.0, 0, 0) ); // --- Shooter configuration and tuning fields --- @@ -16,26 +28,26 @@ public class ShooterConstants { /** Speed for pitching the shooter (open-loop, 0..1). */ // public double shooterPitchSpeed = 0.1; /** Hold speed for maintaining shooter pitch (open-loop, 0..1). */ - public double shooterPitchHoldSpeed = 0.0; + public static final double SHOOTER_PITCH_HOLD_SPEED = 0.0; /** Maximum allowed shooter pitch (units depend on mechanism, e.g., rotations or percent). */ - public double shooterPitchMax = 0.6; + public static final double SHOOTER_PITCH_MAX_SPEED = 0.6; /** Minimum allowed shooter pitch. */ - public double shooterPitchMin = 0.0; + public static final double SHOOTER_PITCH_MIN_SPEED = 0.0; /** Threshold for position manager to consider the shooter "at position". */ - public double positionThreshold = 0.02; + public static final double POSITION_THRESHOLD = 0.02; - public double feederInSpeed = 0.2; - public double feederOutSpeed = -0.05; + public static final double FEEDER_IN_SPEED = 0.2; + public static final double FEEDER_OUT_SPEED = -0.05; /** Height of the hub (target) in meters. */ - public double hubHeight = 6 / 3.281; // 6 feet to meters + public static final double HUB_HEIGHT_METRES = 6 / 3.281; // 6 feet to meters /** Aim above hub in meters */ - public double aimAbove = 1 / 3.281; // 1 foot to meters + public static final double AIM_ABOVE = 1 / 3.281; // 1 foot to meters /** Height of the shooter in meters. */ - public double shooterHeight = 20 / 12 / 3.281; // 26 inches to meters + public static final double SHOOTER_HEIGHT = 20 / 12 / 3.281; // 26 inches to meters } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index bebc8288..c0bd9e98 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -72,7 +72,7 @@ public ShooterSubsystem(Drive drivetrain) { } /** IMU sensor for shooter orientation feedback. */ - public final Pigeon2 shooterPigeon = new Pigeon2(34); + // public final Pigeon2 shooterPigeon = new Pigeon2(34); public final CANcoder shooterThroughBore = new CANcoder(36); @@ -89,6 +89,16 @@ public double getThroughBorePosition() { return shooterThroughBore.getAbsolutePosition().getValueAsDouble() + offset; } + // For testing shooter angle manually + private double shooterAngleTargetTesting = 0.0; + public void incrementShooterAngle(double incrementValue) { + shooterAngleTargetTesting += incrementValue; + + Logger.recordOutput("ShooterSubsystem/TestingAngleTarget", shooterAngleTargetTesting); + + shooterPitchMotor.goTo(shooterAngleTargetTesting); + } + public UpperLowerPoint shooterUpperLower() { // MARK: NEEDS REFACTORING // if (dataPoints.isEmpty()) { diff --git a/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java b/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java index 6e1c595c..bc01fe34 100644 --- a/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.vision.limelight; +import static edu.wpi.first.units.Units.Degrees; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Rotation2d; @@ -24,7 +26,7 @@ public class LimelightConstants { // Position of Limelight relative to the centre of the robot in metres public static final Transform2d LIMELIGHT_4_TRANSFORM_FROM_CENTRE = new Transform2d( new Translation2d(0.0762, 0.228), - new Rotation2d(0) + new Rotation2d(-0.2792526803) ); /** Calculate dynamic standard deviations for Quest */ From 6ed8edfbecaffa7f8dbec388512ccb7f8b73457f Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Tue, 17 Mar 2026 13:27:59 -0500 Subject: [PATCH 049/102] Minor changes --- src/main/java/frc/robot/BuildConstants.java | 12 ++++++------ src/main/java/frc/robot/Robot.java | 8 ++++---- .../mechanisms/shooter/ShooterSubsystem.java | 2 ++ .../vision/limelight/LimelightConstants.java | 2 +- 4 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 3a870fdf..05dd7de0 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,14 +5,14 @@ */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026 Roomba"; + public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 172; - public static final String GIT_SHA = "e822ff25fc2605c0d253fbcad5dd1d725919ddd9"; - public static final String GIT_DATE = "2026-03-17 10:50:37 CDT"; + public static final int GIT_REVISION = 173; + public static final String GIT_SHA = "92059eb0f6fb94850e63c7945c1114c51f68742c"; + public static final String GIT_DATE = "2026-03-17 11:58:41 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-17 11:56:58 CDT"; - public static final long BUILD_UNIX_TIME = 1773766618484L; + public static final String BUILD_DATE = "2026-03-17 13:26:53 CDT"; + public static final long BUILD_UNIX_TIME = 1773772013632L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6e8c724a..e22aaf97 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -110,10 +110,10 @@ public void robotPeriodic() { logDriveStationValues(); - if (!currentAlliance.equals(DriverStation.getAlliance())) { - currentAlliance = DriverStation.getAlliance(); - Logger.recordOutput("FieldInfo/CurrentAlliance", currentAlliance.toString()); - } + // if (!currentAlliance.equals(DriverStation.getAlliance())) { + // currentAlliance = DriverStation.getAlliance(); + // Logger.recordOutput("FieldInfo/CurrentAlliance", currentAlliance.toString()); + // } } // MARK: Disabled Init diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index c0bd9e98..e515c426 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -78,7 +78,9 @@ public ShooterSubsystem(Drive drivetrain) { @Override public void periodic() { + Logger.recordOutput("ShooterSubsystem/MotorConnected", shooterPitchMotor.getMotor().isConnected()); Logger.recordOutput("ShooterSubsystem/ThroughBoreAngle", getThroughBorePosition()); + Logger.recordOutput("ShooterSubsystem/MotorAngle", shooterPitchMotor.getMotor().getPosition().getValueAsDouble()); } // --- Commands --- \\ diff --git a/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java b/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java index bc01fe34..65bfa796 100644 --- a/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java @@ -25,7 +25,7 @@ public class LimelightConstants { // Position of Limelight relative to the centre of the robot in metres public static final Transform2d LIMELIGHT_4_TRANSFORM_FROM_CENTRE = new Transform2d( - new Translation2d(0.0762, 0.228), + new Translation2d(0.0762, -0.26035), new Rotation2d(-0.2792526803) ); From d807059575a5ab8b83f2f528739b7992504d3781 Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Tue, 17 Mar 2026 14:36:15 -0500 Subject: [PATCH 050/102] changes --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- .../mechanisms/shooter/ShooterConstants.java | 5 ++++- .../mechanisms/shooter/ShooterSubsystem.java | 15 ++++++++++----- 3 files changed, 19 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 05dd7de0..7b90f7fa 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 173; - public static final String GIT_SHA = "92059eb0f6fb94850e63c7945c1114c51f68742c"; - public static final String GIT_DATE = "2026-03-17 11:58:41 CDT"; + public static final int GIT_REVISION = 174; + public static final String GIT_SHA = "6ed8edfbecaffa7f8dbec388512ccb7f8b73457f"; + public static final String GIT_DATE = "2026-03-17 13:27:59 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-17 13:26:53 CDT"; - public static final long BUILD_UNIX_TIME = 1773772013632L; + public static final String BUILD_DATE = "2026-03-17 14:35:23 CDT"; + public static final long BUILD_UNIX_TIME = 1773776123317L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java index 9862ce94..095945bd 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java @@ -40,7 +40,10 @@ public class ShooterConstants { public static final double FEEDER_IN_SPEED = 0.2; public static final double FEEDER_OUT_SPEED = -0.05; - + + public static final double SHOOTER_MIN_ANGLE = 5; + + public static final double SHOOTER_MAX_ANGLE = 65; diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index e515c426..8d12f9fb 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -37,15 +37,16 @@ public class ShooterSubsystem extends SubsystemBase { TreeMap dataPoints = new TreeMap<>(); - public ShooterConstants shooterConstants = new ShooterConstants(); + // public ShooterConstants shooterConstants = new ShooterConstants(); // MARK: Motors /** Motor controlling the shooter pitch (angle). */ // this is reversed in real life so its inverted in the code public final Motor shooterPitchMotor = new Motor(11, "Mechanisms", true) .setFree(false) - .setRange(-0.5, 0) - .setMotorSpeed(0.1); + .setRange(ShooterConstants.SHOOTER_MAX_ANGLE, ShooterConstants.SHOOTER_MIN_ANGLE) + .setMotorSpeed(0.1) + .setPositionSupplier(() -> getThroughBorePosition()); public final Motor leftShooterMotor = new Motor(13, "Mechanisms"); public final Motor rightShooterMotor = new Motor(14, "Mechanisms", true); @@ -72,7 +73,7 @@ public ShooterSubsystem(Drive drivetrain) { } /** IMU sensor for shooter orientation feedback. */ - // public final Pigeon2 shooterPigeon = new Pigeon2(34); + public final Pigeon2 shooterPigeon = new Pigeon2(34, "Mechanisms"); public final CANcoder shooterThroughBore = new CANcoder(36); @@ -80,7 +81,7 @@ public ShooterSubsystem(Drive drivetrain) { public void periodic() { Logger.recordOutput("ShooterSubsystem/MotorConnected", shooterPitchMotor.getMotor().isConnected()); Logger.recordOutput("ShooterSubsystem/ThroughBoreAngle", getThroughBorePosition()); - Logger.recordOutput("ShooterSubsystem/MotorAngle", shooterPitchMotor.getMotor().getPosition().getValueAsDouble()); + Logger.recordOutput("ShooterSubsystem/PigeonAngle", getPigeonPosition()); } // --- Commands --- \\ @@ -91,6 +92,10 @@ public double getThroughBorePosition() { return shooterThroughBore.getAbsolutePosition().getValueAsDouble() + offset; } + public double getPigeonPosition() { + return shooterPigeon.getRoll().getValueAsDouble() * -1; + } + // For testing shooter angle manually private double shooterAngleTargetTesting = 0.0; public void incrementShooterAngle(double incrementValue) { From b752860b7344d9d5f5c188dd581ff8f17e9355b2 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 14:40:04 -0500 Subject: [PATCH 051/102] updated some values. fixed brake. --- src/main/java/frc/robot/Robot.java | 34 +++++++------------ .../mechanisms/shooter/ShooterConstants.java | 2 +- .../mechanisms/shooter/ShooterSubsystem.java | 1 + 3 files changed, 15 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e22aaf97..49f32e6c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -97,6 +97,19 @@ public void robotInit() { // Reset motor speeds to zero. robotContainer.intakeSubsystem.angleMotor.set(0.0); robotContainer.shooterSubsystem.shooterPitchMotor.set(0.0); + + NeutralModeValue settingNeutralModeValue = NeutralModeValue.Brake; + robotContainer.feederSubsystem.feederBedMotor.setNeutralMode(settingNeutralModeValue); + robotContainer.feederSubsystem.feederFeederMotor.setNeutralMode(settingNeutralModeValue); + robotContainer.feederSubsystem.shooterFeederMotor.setNeutralMode(settingNeutralModeValue); + + robotContainer.shooterSubsystem.shooterPitchMotor.setNeutralMode(settingNeutralModeValue); + + robotContainer.intakeSubsystem.angleMotor.setNeutralMode(settingNeutralModeValue); + robotContainer.intakeSubsystem.intakeWheelsMotor.setNeutralMode(settingNeutralModeValue); + + robotContainer.shooterSubsystem.leftShooterMotor.setNeutralMode(NeutralModeValue.Coast); + robotContainer.shooterSubsystem.rightShooterMotor.setNeutralMode(NeutralModeValue.Coast); } // MARK: Robot Periodic @@ -119,15 +132,6 @@ public void robotPeriodic() { // MARK: Disabled Init @Override public void disabledInit() { - NeutralModeValue settingNeutralModeValue = NeutralModeValue.Coast; - robotContainer.feederSubsystem.feederBedMotor.setNeutralMode(settingNeutralModeValue); - robotContainer.feederSubsystem.feederFeederMotor.setNeutralMode(settingNeutralModeValue); - robotContainer.feederSubsystem.shooterFeederMotor.setNeutralMode(settingNeutralModeValue); - - robotContainer.shooterSubsystem.shooterPitchMotor.setNeutralMode(settingNeutralModeValue); - - robotContainer.intakeSubsystem.angleMotor.setNeutralMode(settingNeutralModeValue); - robotContainer.intakeSubsystem.intakeWheelsMotor.setNeutralMode(settingNeutralModeValue); } // MARK: Disabled Periodic @@ -137,18 +141,6 @@ public void disabledPeriodic() {} // MARK: Disabled Exit @Override public void disabledExit() { - NeutralModeValue settingNeutralModeValue = NeutralModeValue.Brake; - robotContainer.feederSubsystem.feederBedMotor.setNeutralMode(settingNeutralModeValue); - robotContainer.feederSubsystem.feederFeederMotor.setNeutralMode(settingNeutralModeValue); - robotContainer.feederSubsystem.shooterFeederMotor.setNeutralMode(settingNeutralModeValue); - - robotContainer.shooterSubsystem.shooterPitchMotor.setNeutralMode(settingNeutralModeValue); - - robotContainer.intakeSubsystem.angleMotor.setNeutralMode(settingNeutralModeValue); - robotContainer.intakeSubsystem.intakeWheelsMotor.setNeutralMode(settingNeutralModeValue); - - robotContainer.shooterSubsystem.leftShooterMotor.setNeutralMode(NeutralModeValue.Coast); - robotContainer.shooterSubsystem.rightShooterMotor.setNeutralMode(NeutralModeValue.Coast); } // MARK: Autonomous Init diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java index 095945bd..a4ceeb5b 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java @@ -36,7 +36,7 @@ public class ShooterConstants { public static final double SHOOTER_PITCH_MIN_SPEED = 0.0; /** Threshold for position manager to consider the shooter "at position". */ - public static final double POSITION_THRESHOLD = 0.02; + public static final double POSITION_THRESHOLD = 1.0; public static final double FEEDER_IN_SPEED = 0.2; public static final double FEEDER_OUT_SPEED = -0.05; diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 8d12f9fb..bd9d6624 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -46,6 +46,7 @@ public class ShooterSubsystem extends SubsystemBase { .setFree(false) .setRange(ShooterConstants.SHOOTER_MAX_ANGLE, ShooterConstants.SHOOTER_MIN_ANGLE) .setMotorSpeed(0.1) + .setThreshold(ShooterConstants.POSITION_THRESHOLD) .setPositionSupplier(() -> getThroughBorePosition()); public final Motor leftShooterMotor = new Motor(13, "Mechanisms"); From 3f4f158a8b87db17675b53e03db4aa6e0f47eeee Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Tue, 17 Mar 2026 14:48:36 -0500 Subject: [PATCH 052/102] changes --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/joysticks/DebugJoystick.java | 4 ++-- .../mechanisms/shooter/ShooterSubsystem.java | 10 +++++----- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 7b90f7fa..345738b4 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 174; - public static final String GIT_SHA = "6ed8edfbecaffa7f8dbec388512ccb7f8b73457f"; - public static final String GIT_DATE = "2026-03-17 13:27:59 CDT"; + public static final int GIT_REVISION = 176; + public static final String GIT_SHA = "b752860b7344d9d5f5c188dd581ff8f17e9355b2"; + public static final String GIT_DATE = "2026-03-17 14:40:04 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-17 14:35:23 CDT"; - public static final long BUILD_UNIX_TIME = 1773776123317L; + public static final String BUILD_DATE = "2026-03-17 14:48:25 CDT"; + public static final long BUILD_UNIX_TIME = 1773776905597L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 710070c2..a824100b 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -89,7 +89,7 @@ public void configureBindings() { joystick.x().whileTrue( Commands.run( () -> { - shooterSubsystem.incrementShooterAngle(0.01); + shooterSubsystem.incrementShooterAngle(0.1); } ) ); @@ -97,7 +97,7 @@ public void configureBindings() { joystick.y().whileTrue( Commands.run( () -> { - shooterSubsystem.incrementShooterAngle(-0.01); + shooterSubsystem.incrementShooterAngle(-0.1); } ) );; diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index bd9d6624..a7a809e3 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -44,10 +44,10 @@ public class ShooterSubsystem extends SubsystemBase { // this is reversed in real life so its inverted in the code public final Motor shooterPitchMotor = new Motor(11, "Mechanisms", true) .setFree(false) - .setRange(ShooterConstants.SHOOTER_MAX_ANGLE, ShooterConstants.SHOOTER_MIN_ANGLE) + .setRange(ShooterConstants.SHOOTER_MIN_ANGLE, ShooterConstants.SHOOTER_MAX_ANGLE) .setMotorSpeed(0.1) .setThreshold(ShooterConstants.POSITION_THRESHOLD) - .setPositionSupplier(() -> getThroughBorePosition()); + .setPositionSupplier(() -> getPigeonPosition()); public final Motor leftShooterMotor = new Motor(13, "Mechanisms"); public final Motor rightShooterMotor = new Motor(14, "Mechanisms", true); @@ -78,8 +78,11 @@ public ShooterSubsystem(Drive drivetrain) { public final CANcoder shooterThroughBore = new CANcoder(36); + public double shooterAngleTargetTesting = 65; + @Override public void periodic() { + Logger.recordOutput("ShooterSubsystem/TestingAngleTarget", shooterAngleTargetTesting); Logger.recordOutput("ShooterSubsystem/MotorConnected", shooterPitchMotor.getMotor().isConnected()); Logger.recordOutput("ShooterSubsystem/ThroughBoreAngle", getThroughBorePosition()); Logger.recordOutput("ShooterSubsystem/PigeonAngle", getPigeonPosition()); @@ -98,12 +101,9 @@ public double getPigeonPosition() { } // For testing shooter angle manually - private double shooterAngleTargetTesting = 0.0; public void incrementShooterAngle(double incrementValue) { shooterAngleTargetTesting += incrementValue; - Logger.recordOutput("ShooterSubsystem/TestingAngleTarget", shooterAngleTargetTesting); - shooterPitchMotor.goTo(shooterAngleTargetTesting); } From 7afda358b479ae2bbc82177cf512d79e1054a1ef Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 14:51:08 -0500 Subject: [PATCH 053/102] stuff --- src/main/java/frc/robot/joysticks/DebugJoystick.java | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index a824100b..973f6c22 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -89,18 +89,20 @@ public void configureBindings() { joystick.x().whileTrue( Commands.run( () -> { - shooterSubsystem.incrementShooterAngle(0.1); - } + shooterPitch = MathUtil.clamp(shooterPitch + 0.0025, -0.5, 0.0); + shooterSubsystem.shooterPitchMotor.goTo(shooterPitch); + }, shooterSubsystem ) ); joystick.y().whileTrue( Commands.run( () -> { - shooterSubsystem.incrementShooterAngle(-0.1); - } + shooterPitch = MathUtil.clamp(shooterPitch - 0.0025, -0.5, 0.0); + shooterSubsystem.shooterPitchMotor.goTo(shooterPitch); + }, shooterSubsystem ) - );; + ); joystick.rightTrigger().whileTrue( NamedCommands.getCommand("RunAllFeederIn") From 0cf12145b47a6f8192e65adf800ed89570608b3e Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 14:53:31 -0500 Subject: [PATCH 054/102] changes --- src/main/java/frc/robot/joysticks/DebugJoystick.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 973f6c22..ed2ff97d 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -98,8 +98,7 @@ public void configureBindings() { joystick.y().whileTrue( Commands.run( () -> { - shooterPitch = MathUtil.clamp(shooterPitch - 0.0025, -0.5, 0.0); - shooterSubsystem.shooterPitchMotor.goTo(shooterPitch); + shooterSubsystem.shooterPitchMotor.goTo(50); }, shooterSubsystem ) ); From 4c439223ad273d57a87ed81f33e0051a7e34a6ab Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 15:02:21 -0500 Subject: [PATCH 055/102] Added shooter acceleration --- .../java/frc/robot/joysticks/DebugJoystick.java | 7 +------ .../mechanisms/shooter/ShooterSubsystem.java | 15 +++++++++++++++ 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index ed2ff97d..599ca493 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -87,12 +87,7 @@ public void configureBindings() { ); joystick.x().whileTrue( - Commands.run( - () -> { - shooterPitch = MathUtil.clamp(shooterPitch + 0.0025, -0.5, 0.0); - shooterSubsystem.shooterPitchMotor.goTo(shooterPitch); - }, shooterSubsystem - ) + shooterSubsystem.accelerateToSpeed(0.5) ); joystick.y().whileTrue( diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index a7a809e3..f6e46da8 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -11,6 +11,9 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotContainer; import frc.robot.subsystems.drive.Drive; @@ -152,6 +155,18 @@ public ShooterDataPoint calculateShooterValues(UpperLowerPoint upperLowerPoint, // return new ShooterDataPoint(currentDistance, estimatedAngle, estimatedSpeed); } + public Command accelerateToSpeed(double targetSpeed) { + Timer timer = new Timer(); + return Commands.startRun( + () -> timer.restart(), + () -> { + double rampedSpeed = Math.min(timer.get() / 5.0, 1.0) * targetSpeed; + shooterMotors.drive(rampedSpeed); + }, + this + ).until(() -> timer.hasElapsed(5.0)); + } + public double getRequiredRPM(ShooterDataPoint shooterDataPoint){ double vWheel = 2 * shooterDataPoint.speed; return vWheel * 60 / (Math.PI * 4 * 0.0254); // get rpm required for wheel with diameter of 4 inches From c3c1ddc442ab44a9406059d32bbb6b343a27d71c Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 15:12:16 -0500 Subject: [PATCH 056/102] things --- src/main/java/frc/robot/joysticks/DebugJoystick.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 599ca493..352a030b 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -57,7 +57,6 @@ public DebugJoystick( shooterPitch = MathUtil.clamp(shooterPitch + Math.signum(rightJoystickValue) * changeAmountPerTick, 0.0 ,0.5); shooterSubsystem.shooterMotors.drive(shooterSpeed); - shooterSubsystem.shooterPitchMotor.goTo(shooterPitch); Logger.recordOutput("ShooterSubsystem/ShooterSpeed", shooterSpeed); Logger.recordOutput("ShooterSubsystem/FeederSpeed", rightJoystickValue); From c27c2f95a96f37c9279b80f38b45f9d9962e0a63 Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Tue, 17 Mar 2026 15:33:44 -0500 Subject: [PATCH 057/102] crying --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/Robot.java | 13 ------------- src/main/java/frc/robot/RobotContainer.java | 6 ++++++ .../java/frc/robot/joysticks/DebugJoystick.java | 10 ++++++++-- .../mechanisms/shooter/ShooterSubsystem.java | 4 ++++ 5 files changed, 23 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 345738b4..73df2342 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 176; - public static final String GIT_SHA = "b752860b7344d9d5f5c188dd581ff8f17e9355b2"; - public static final String GIT_DATE = "2026-03-17 14:40:04 CDT"; + public static final int GIT_REVISION = 181; + public static final String GIT_SHA = "c3c1ddc442ab44a9406059d32bbb6b343a27d71c"; + public static final String GIT_DATE = "2026-03-17 15:12:16 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-17 14:48:25 CDT"; - public static final long BUILD_UNIX_TIME = 1773776905597L; + public static final String BUILD_DATE = "2026-03-17 15:32:58 CDT"; + public static final long BUILD_UNIX_TIME = 1773779578574L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 49f32e6c..cb78c383 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -97,19 +97,6 @@ public void robotInit() { // Reset motor speeds to zero. robotContainer.intakeSubsystem.angleMotor.set(0.0); robotContainer.shooterSubsystem.shooterPitchMotor.set(0.0); - - NeutralModeValue settingNeutralModeValue = NeutralModeValue.Brake; - robotContainer.feederSubsystem.feederBedMotor.setNeutralMode(settingNeutralModeValue); - robotContainer.feederSubsystem.feederFeederMotor.setNeutralMode(settingNeutralModeValue); - robotContainer.feederSubsystem.shooterFeederMotor.setNeutralMode(settingNeutralModeValue); - - robotContainer.shooterSubsystem.shooterPitchMotor.setNeutralMode(settingNeutralModeValue); - - robotContainer.intakeSubsystem.angleMotor.setNeutralMode(settingNeutralModeValue); - robotContainer.intakeSubsystem.intakeWheelsMotor.setNeutralMode(settingNeutralModeValue); - - robotContainer.shooterSubsystem.leftShooterMotor.setNeutralMode(NeutralModeValue.Coast); - robotContainer.shooterSubsystem.rightShooterMotor.setNeutralMode(NeutralModeValue.Coast); } // MARK: Robot Periodic diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9fbdc7a6..0178ba34 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,7 +9,9 @@ import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; @@ -63,6 +65,10 @@ public class RobotContainer { .withSupplyCurrentLimit(Amps.of(40)) .withStatorCurrentLimitEnable(true) .withSupplyCurrentLimitEnable(true) + ) + .withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) ); // MARK: Tests diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 352a030b..15d171cb 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -54,7 +54,7 @@ public DebugJoystick( double change = Math.signum(leftJoystickValue) * changeAmountPerTick; shooterSpeed = MathUtil.clamp(shooterSpeed + change, -0.1, 1.0); - shooterPitch = MathUtil.clamp(shooterPitch + Math.signum(rightJoystickValue) * changeAmountPerTick, 0.0 ,0.5); + // shooterPitch = MathUtil.clamp(shooterPitch + Math.signum(rightJoystickValue) * changeAmountPerTick, 0.0 ,0.5); shooterSubsystem.shooterMotors.drive(shooterSpeed); @@ -86,7 +86,13 @@ public void configureBindings() { ); joystick.x().whileTrue( - shooterSubsystem.accelerateToSpeed(0.5) + // shooterSubsystem.accelerateToSpeed(0.5) + Commands.run( + () -> { + shooterSubsystem.leftShooterMotor.getMotor().set(0.5); + shooterSubsystem.leftShooterMotor.getMotor().set(-0.5); + } + ) ); joystick.y().whileTrue( diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index f6e46da8..197f35d4 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -71,6 +71,10 @@ public ShooterSubsystem(Drive drivetrain) { leftShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); rightShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + shooterPitchMotor.toggleEnabled(true); + leftShooterMotor.toggleEnabled(true); + rightShooterMotor.toggleEnabled(true); + for (ShooterDataPoint point : ShooterConstants.shooterDataPoints) { dataPoints.put(point.distance, point); } From 44858626661a21ba36775b3f01a0da9a9f99fc69 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 15:36:34 -0500 Subject: [PATCH 058/102] s --- src/main/java/frc/robot/Robot.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cb78c383..1ce97677 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -97,6 +97,11 @@ public void robotInit() { // Reset motor speeds to zero. robotContainer.intakeSubsystem.angleMotor.set(0.0); robotContainer.shooterSubsystem.shooterPitchMotor.set(0.0); + + // Re-apply Phoenix 6 configuration last so WhatTime initialization cannot overwrite it. + robotContainer.shooterSubsystem.shooterPitchMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + robotContainer.shooterSubsystem.leftShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + robotContainer.shooterSubsystem.rightShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); } // MARK: Robot Periodic From 3723b8a6bc69c54cd1747310845bc379d3c881eb Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 15:50:32 -0500 Subject: [PATCH 059/102] sdfgdsfg --- src/main/java/frc/robot/Robot.java | 14 +++++++------- .../mechanisms/shooter/ShooterSubsystem.java | 5 ----- 2 files changed, 7 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1ce97677..28cbf101 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -91,17 +91,17 @@ public void robotInit() { DriverStation.silenceJoystickConnectionWarning(true); - robotContainer.shooterSubsystem.shooterPitchMotor.setNeutralMode(NeutralModeValue.Brake); - robotContainer.intakeSubsystem.angleMotor.setNeutralMode(NeutralModeValue.Brake); + // Enable motors (WhatTime Motor.isEnabled defaults to false — drive/goTo do nothing until this is called) + robotContainer.shooterSubsystem.shooterPitchMotor.toggleEnabled(true); + robotContainer.shooterSubsystem.leftShooterMotor.toggleEnabled(true); + robotContainer.shooterSubsystem.rightShooterMotor.toggleEnabled(true); + robotContainer.intakeSubsystem.angleMotor.toggleEnabled(true); - // Reset motor speeds to zero. - robotContainer.intakeSubsystem.angleMotor.set(0.0); - robotContainer.shooterSubsystem.shooterPitchMotor.set(0.0); - - // Re-apply Phoenix 6 configuration last so WhatTime initialization cannot overwrite it. + // Apply full motor config here (after CAN bus is stable — constructor-time apply() silently fails) robotContainer.shooterSubsystem.shooterPitchMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); robotContainer.shooterSubsystem.leftShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); robotContainer.shooterSubsystem.rightShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + robotContainer.intakeSubsystem.angleMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); } // MARK: Robot Periodic diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 197f35d4..e70414e0 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.RobotContainer; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.math.MathSubsystem; @@ -67,10 +66,6 @@ public class ShooterSubsystem extends SubsystemBase { public ShooterSubsystem(Drive drivetrain) { this.drivetrain = drivetrain; - shooterPitchMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); - leftShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); - rightShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); - shooterPitchMotor.toggleEnabled(true); leftShooterMotor.toggleEnabled(true); rightShooterMotor.toggleEnabled(true); From b4cc61d5e1a2de1e592247331c12baae4af70f4f Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 15:54:46 -0500 Subject: [PATCH 060/102] asdfasdf --- src/main/java/frc/robot/joysticks/DebugJoystick.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 15d171cb..1f3faf0b 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -54,7 +54,7 @@ public DebugJoystick( double change = Math.signum(leftJoystickValue) * changeAmountPerTick; shooterSpeed = MathUtil.clamp(shooterSpeed + change, -0.1, 1.0); - // shooterPitch = MathUtil.clamp(shooterPitch + Math.signum(rightJoystickValue) * changeAmountPerTick, 0.0 ,0.5); + shooterPitch = MathUtil.clamp(shooterPitch + Math.signum(rightJoystickValue) * changeAmountPerTick, 5.0 ,65.0); shooterSubsystem.shooterMotors.drive(shooterSpeed); From 98de02ddac42ab9552dccb0092a717e54ce6f130 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 16:04:10 -0500 Subject: [PATCH 061/102] maybe fixed brake issues --- src/main/java/frc/robot/Robot.java | 9 +++++---- src/main/java/frc/robot/RobotContainer.java | 1 + src/main/java/frc/robot/joysticks/DebugJoystick.java | 9 ++++----- .../subsystems/mechanisms/feeder/FeederSubsystem.java | 4 ++++ .../subsystems/mechanisms/intake/IntakeSubsystem.java | 3 +++ 5 files changed, 17 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 28cbf101..bb87a16e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -98,10 +98,11 @@ public void robotInit() { robotContainer.intakeSubsystem.angleMotor.toggleEnabled(true); // Apply full motor config here (after CAN bus is stable — constructor-time apply() silently fails) - robotContainer.shooterSubsystem.shooterPitchMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); - robotContainer.shooterSubsystem.leftShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); - robotContainer.shooterSubsystem.rightShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); - robotContainer.intakeSubsystem.angleMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + // StatusCodes are logged so you can verify success in AdvantageScope under MotorConfig/ + Logger.recordOutput("MotorConfig/ShooterPitch", robotContainer.shooterSubsystem.shooterPitchMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + Logger.recordOutput("MotorConfig/LeftShooter", robotContainer.shooterSubsystem.leftShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + Logger.recordOutput("MotorConfig/RightShooter", robotContainer.shooterSubsystem.rightShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + Logger.recordOutput("MotorConfig/IntakeAngle", robotContainer.intakeSubsystem.angleMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); } // MARK: Robot Periodic diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0178ba34..ca9888a5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -69,6 +69,7 @@ public class RobotContainer { .withMotorOutput( new MotorOutputConfigs() .withNeutralMode(NeutralModeValue.Brake) + .withDutyCycleNeutralDeadband(0.04) ); // MARK: Tests diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 1f3faf0b..879b3766 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.mechanisms.feeder.FeederState; import frc.robot.subsystems.mechanisms.feeder.FeederSubsystem; import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; @@ -85,12 +86,10 @@ public void configureBindings() { NamedCommands.getCommand("IntakeUp") ); - joystick.x().whileTrue( - // shooterSubsystem.accelerateToSpeed(0.5) - Commands.run( + joystick.x().onTrue( + Commands.runOnce( () -> { - shooterSubsystem.leftShooterMotor.getMotor().set(0.5); - shooterSubsystem.leftShooterMotor.getMotor().set(-0.5); + feederSubsystem.setFeederState(FeederState.ALL_FEEDER_IN); } ) ); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java index 632dcacc..41decc7f 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java @@ -25,6 +25,10 @@ public class FeederSubsystem extends SubsystemBase { // Applys current limits to motors to reduce chance of brownout // MARK: Constructor public FeederSubsystem() { + feederBedMotor.toggleEnabled(true); + feederFeederMotor.toggleEnabled(true); + shooterFeederMotor.toggleEnabled(true); + feederBedMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); feederFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); shooterFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 8c3ab16b..78198130 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -24,6 +24,9 @@ public class IntakeSubsystem extends SubsystemBase { .setPositionSupplier(()-> angleMotor.getCurrentValue()); public IntakeSubsystem() { + angleMotor.toggleEnabled(true); + intakeWheelsMotor.toggleEnabled(true); + angleMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); intakeWheelsMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); } From bd1b693c6229fbc93e2df9e01d99764cffab260e Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 16:14:07 -0500 Subject: [PATCH 062/102] asdfsafd --- src/main/java/frc/robot/joysticks/DebugJoystick.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 879b3766..af152ab3 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -94,11 +94,11 @@ public void configureBindings() { ) ); - joystick.y().whileTrue( - Commands.run( + joystick.y().onTrue( + Commands.runOnce( () -> { - shooterSubsystem.shooterPitchMotor.goTo(50); - }, shooterSubsystem + feederSubsystem.setFeederState(FeederState.OFF); + } ) ); From 30d0aaec33d6359e4f5b27c71c69db3b331dca5a Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 16:20:52 -0500 Subject: [PATCH 063/102] maybe fixed stuff --- src/main/java/frc/robot/Robot.java | 22 +++++++++++++------ .../mechanisms/feeder/FeederSubsystem.java | 2 +- .../mechanisms/shooter/ShooterSubsystem.java | 4 ++-- 3 files changed, 18 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bb87a16e..4f014f59 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -96,20 +96,26 @@ public void robotInit() { robotContainer.shooterSubsystem.leftShooterMotor.toggleEnabled(true); robotContainer.shooterSubsystem.rightShooterMotor.toggleEnabled(true); robotContainer.intakeSubsystem.angleMotor.toggleEnabled(true); + robotContainer.intakeSubsystem.intakeWheelsMotor.toggleEnabled(true); + robotContainer.feederSubsystem.feederBedMotor.toggleEnabled(true); + robotContainer.feederSubsystem.feederFeederMotor.toggleEnabled(true); + robotContainer.feederSubsystem.shooterFeederMotor.toggleEnabled(true); // Apply full motor config here (after CAN bus is stable — constructor-time apply() silently fails) // StatusCodes are logged so you can verify success in AdvantageScope under MotorConfig/ - Logger.recordOutput("MotorConfig/ShooterPitch", robotContainer.shooterSubsystem.shooterPitchMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); - Logger.recordOutput("MotorConfig/LeftShooter", robotContainer.shooterSubsystem.leftShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); - Logger.recordOutput("MotorConfig/RightShooter", robotContainer.shooterSubsystem.rightShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); - Logger.recordOutput("MotorConfig/IntakeAngle", robotContainer.intakeSubsystem.angleMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + Logger.recordOutput("MotorConfig/ShooterPitch", robotContainer.shooterSubsystem.shooterPitchMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + Logger.recordOutput("MotorConfig/LeftShooter", robotContainer.shooterSubsystem.leftShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + Logger.recordOutput("MotorConfig/RightShooter", robotContainer.shooterSubsystem.rightShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + Logger.recordOutput("MotorConfig/IntakeAngle", robotContainer.intakeSubsystem.angleMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + Logger.recordOutput("MotorConfig/IntakeWheels", robotContainer.intakeSubsystem.intakeWheelsMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + Logger.recordOutput("MotorConfig/FeederBed", robotContainer.feederSubsystem.feederBedMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + Logger.recordOutput("MotorConfig/FeederFeeder", robotContainer.feederSubsystem.feederFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + Logger.recordOutput("MotorConfig/ShooterFeeder", robotContainer.feederSubsystem.shooterFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); } // MARK: Robot Periodic @Override public void robotPeriodic() { - pdp.clearStickyFaults(); - m_timeAndJoystickReplay.update(); CommandScheduler.getInstance().run(); @@ -129,7 +135,9 @@ public void disabledInit() { // MARK: Disabled Periodic @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + pdp.clearStickyFaults(); + } // MARK: Disabled Exit @Override diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java index 41decc7f..6d0ce73f 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java @@ -33,7 +33,7 @@ public FeederSubsystem() { feederFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); shooterFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); - motorBedAgitationRoutine(); + motorBedAgitationRoutine().schedule(); } // MARK: Periodic Loop diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index e70414e0..8081aeb4 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -95,11 +95,11 @@ public void periodic() { // MARK: Get Through Bore public double getThroughBorePosition() { double offset = 0.4; - return shooterThroughBore.getAbsolutePosition().getValueAsDouble() + offset; + return shooterThroughBore.getAbsolutePosition().refresh().getValueAsDouble() + offset; } public double getPigeonPosition() { - return shooterPigeon.getRoll().getValueAsDouble() * -1; + return shooterPigeon.getRoll().refresh().getValueAsDouble() * -1; } // For testing shooter angle manually From ac56c057593bf8754d9c8e373c5d45d6dc52680b Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 16:27:48 -0500 Subject: [PATCH 064/102] aasdf --- .../mechanisms/feeder/FeederSubsystem.java | 45 ++++++++++--------- 1 file changed, 23 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java index 6d0ce73f..85132741 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java @@ -3,8 +3,7 @@ import org.littletonrobotics.junction.Logger; import com.btwrobotics.WhatTime.frc.MotorManagers.Motor; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotContainer; @@ -22,7 +21,11 @@ public class FeederSubsystem extends SubsystemBase { // MARK: Feeder State private FeederState feederState = FeederState.OFF; - // Applys current limits to motors to reduce chance of brownout + // MARK: Bed Agitation Timer + private final Timer bedAgitationTimer = new Timer(); + private static final double BED_FORWARD_SECONDS = 10.0; + private static final double BED_REVERSE_SECONDS = 2.0; + // MARK: Constructor public FeederSubsystem() { feederBedMotor.toggleEnabled(true); @@ -33,17 +36,19 @@ public FeederSubsystem() { feederFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); shooterFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); - motorBedAgitationRoutine().schedule(); + bedAgitationTimer.start(); } // MARK: Periodic Loop @Override public void periodic() { + runBedAgitation(); + switch (feederState) { case ALL_FEEDER_IN: runSpecifiedMotors(true, false, true, false); break; - + case ALL_FEEDER_OUT: runSpecifiedMotors(true, true, true, true); break; @@ -55,11 +60,11 @@ public void periodic() { case SHOOTER_FEED_OUT: runSpecifiedMotors(true, true, true, true); break; - + case OFF: runSpecifiedMotors(false, false, false, false); break; - + default: runSpecifiedMotors(false, false, false, false); break; @@ -67,24 +72,20 @@ public void periodic() { } // MARK: Bed Agitation - // Reverses the bed roller motors every 10 seconds - private Command motorBedAgitationRoutine() { - return Commands.repeatingSequence( - Commands.run( - () -> { - feederBedMotor.drive(); - } - ).withTimeout(10), - Commands.run( - () -> { - feederBedMotor.drive(true); - } - ).withTimeout(2) - ); + // Drives forward for 10 seconds, reverses for 2 seconds, repeats. + private void runBedAgitation() { + double t = bedAgitationTimer.get() % (BED_FORWARD_SECONDS + BED_REVERSE_SECONDS); + if (t < BED_FORWARD_SECONDS) { + feederBedMotor.drive(); + } else { + feederBedMotor.drive(true); + } + + Logger.recordOutput("FeederSubsystem/BedAgitationTime", t); } private void runSpecifiedMotors( - boolean runFeederFeeder, + boolean runFeederFeeder, boolean feederFeederInverted, boolean runShooterFeeder, boolean shooterFeederInverted From 90cca78f786acf66ef862924d657c55332a6640e Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Tue, 17 Mar 2026 16:36:13 -0500 Subject: [PATCH 065/102] Changes --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- .../subsystems/mechanisms/feeder/FeederConstants.java | 2 +- .../subsystems/mechanisms/feeder/FeederSubsystem.java | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 73df2342..40cd1549 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 181; - public static final String GIT_SHA = "c3c1ddc442ab44a9406059d32bbb6b343a27d71c"; - public static final String GIT_DATE = "2026-03-17 15:12:16 CDT"; + public static final int GIT_REVISION = 189; + public static final String GIT_SHA = "ac56c057593bf8754d9c8e373c5d45d6dc52680b"; + public static final String GIT_DATE = "2026-03-17 16:27:48 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-17 15:32:58 CDT"; - public static final long BUILD_UNIX_TIME = 1773779578574L; + public static final String BUILD_DATE = "2026-03-17 16:29:49 CDT"; + public static final long BUILD_UNIX_TIME = 1773782989929L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java index 5067ced9..5cb80cd4 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.mechanisms.feeder; public class FeederConstants { - public static double FEEDER_BED_SPEED = 0.5; + public static double FEEDER_BED_SPEED = 0.2; public static double FEEDER_FEEDER_SPEED = 0.5; public static double SHOOTER_FEEDER_SPEED = 0.5; } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java index 85132741..c490c20d 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java @@ -10,7 +10,7 @@ public class FeederSubsystem extends SubsystemBase { // MARK: Feeder Bed - public Motor feederBedMotor = new Motor(15, "Mechanisms").setFree(true).setMotorSpeed(FeederConstants.FEEDER_BED_SPEED); + public Motor feederBedMotor = new Motor(15, "Mechanisms", true).setFree(true).setMotorSpeed(FeederConstants.FEEDER_BED_SPEED); // MARK: Feeder Feeder public Motor feederFeederMotor = new Motor(16, "Mechanisms").setFree(true).setMotorSpeed(FeederConstants.FEEDER_FEEDER_SPEED); From 78e10efa1a71f73a448ea3890cb1ad9842e93ddc Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 16:42:24 -0500 Subject: [PATCH 066/102] aiufh --- .../mechanisms/feeder/FeederSubsystem.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java index c490c20d..9e817886 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java @@ -90,15 +90,15 @@ private void runSpecifiedMotors( boolean runShooterFeeder, boolean shooterFeederInverted ) { - feederFeederMotor.toggleEnabled(runFeederFeeder); - shooterFeederMotor.toggleEnabled(runShooterFeeder); + double feederSpeed = runFeederFeeder ? (feederFeederInverted ? -FeederConstants.FEEDER_FEEDER_SPEED : FeederConstants.FEEDER_FEEDER_SPEED) : 0.0; + double shooterSpeed = runShooterFeeder ? (shooterFeederInverted ? -FeederConstants.SHOOTER_FEEDER_SPEED : FeederConstants.SHOOTER_FEEDER_SPEED) : 0.0; - if (runFeederFeeder) { - feederFeederMotor.drive(feederFeederInverted); - } - if (runShooterFeeder) { - shooterFeederMotor.drive(shooterFeederInverted); - } + feederFeederMotor.getMotor().set(feederSpeed); + shooterFeederMotor.getMotor().set(shooterSpeed); + + Logger.recordOutput("FeederSubsystem/FeederFeederSpeed", feederSpeed); + Logger.recordOutput("FeederSubsystem/ShooterFeederSpeed", shooterSpeed); + Logger.recordOutput("FeederSubsystem/FeederState", feederState.toString()); } public void setFeederState(FeederState feederState) { From d6152bc955495135b7eb15326aaabc1b69756d38 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 16:45:45 -0500 Subject: [PATCH 067/102] ugh --- src/main/java/frc/robot/RobotContainer.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ca9888a5..6b23146d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -107,6 +107,7 @@ public RobotContainer() { private void configureBindings() { driverJoystick.configureBindings(); operatorJoystick.configureBindings(); + debugJoystick.configureBindings(); drivetrain.setDefaultCommand(drivetrain.joysticksDefaultCommand(driverJoystick.joystick)); From f948b547e2d749e38b8d1c57806164cd31cdbead Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 16:53:53 -0500 Subject: [PATCH 068/102] asdfasdf --- .../robot/subsystems/mechanisms/feeder/FeederSubsystem.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java index 9e817886..e9df052d 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java @@ -4,6 +4,7 @@ import com.btwrobotics.WhatTime.frc.MotorManagers.Motor; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotContainer; @@ -36,6 +37,11 @@ public FeederSubsystem() { feederFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); shooterFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + // WhatTime's Motor default command calls set(0) every loop after periodic() runs, + // overwriting the direct TalonFX.set() calls in runSpecifiedMotors. Replace with a no-op. + feederFeederMotor.setDefaultCommand(Commands.run(() -> {}, feederFeederMotor)); + shooterFeederMotor.setDefaultCommand(Commands.run(() -> {}, shooterFeederMotor)); + bedAgitationTimer.start(); } From ccc02c62e0f2bb7ae54a362b635224abbe9098ef Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Tue, 17 Mar 2026 17:06:49 -0500 Subject: [PATCH 069/102] things --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/joysticks/DebugJoystick.java | 6 +++++- .../subsystems/mechanisms/feeder/FeederConstants.java | 6 +++--- .../subsystems/mechanisms/feeder/FeederSubsystem.java | 2 -- 4 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 40cd1549..7144003c 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 189; - public static final String GIT_SHA = "ac56c057593bf8754d9c8e373c5d45d6dc52680b"; - public static final String GIT_DATE = "2026-03-17 16:27:48 CDT"; + public static final int GIT_REVISION = 193; + public static final String GIT_SHA = "f948b547e2d749e38b8d1c57806164cd31cdbead"; + public static final String GIT_DATE = "2026-03-17 16:53:53 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-17 16:29:49 CDT"; - public static final long BUILD_UNIX_TIME = 1773782989929L; + public static final String BUILD_DATE = "2026-03-17 17:05:46 CDT"; + public static final long BUILD_UNIX_TIME = 1773785146530L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index af152ab3..3f97ca4e 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -83,7 +83,11 @@ public void configureBindings() { ); joystick.b().onTrue( - NamedCommands.getCommand("IntakeUp") + Commands.runOnce( + () -> { + feederSubsystem.setFeederState(FeederState.ALL_FEEDER_OUT); + } + ) ); joystick.x().onTrue( diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java index 5cb80cd4..27a30bc6 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederConstants.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.mechanisms.feeder; public class FeederConstants { - public static double FEEDER_BED_SPEED = 0.2; - public static double FEEDER_FEEDER_SPEED = 0.5; - public static double SHOOTER_FEEDER_SPEED = 0.5; + public static final double FEEDER_BED_SPEED = 0.2; + public static final double FEEDER_FEEDER_SPEED = 0.5; + public static final double SHOOTER_FEEDER_SPEED = 0.75; } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java index e9df052d..e7631c8b 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java @@ -37,8 +37,6 @@ public FeederSubsystem() { feederFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); shooterFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); - // WhatTime's Motor default command calls set(0) every loop after periodic() runs, - // overwriting the direct TalonFX.set() calls in runSpecifiedMotors. Replace with a no-op. feederFeederMotor.setDefaultCommand(Commands.run(() -> {}, feederFeederMotor)); shooterFeederMotor.setDefaultCommand(Commands.run(() -> {}, shooterFeederMotor)); From dfe769b55a4d7e9b1877ad5e37bbfbf77b0e9beb Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Tue, 17 Mar 2026 17:58:37 -0500 Subject: [PATCH 070/102] asdfsadfsadfsadf --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- .../robot/commands/namedcommands/RegisterCommands.java | 5 +++-- .../subsystems/mechanisms/feeder/FeederSubsystem.java | 1 + .../subsystems/mechanisms/intake/IntakeConstants.java | 4 ++-- 4 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 7144003c..92d5f33b 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 193; - public static final String GIT_SHA = "f948b547e2d749e38b8d1c57806164cd31cdbead"; - public static final String GIT_DATE = "2026-03-17 16:53:53 CDT"; + public static final int GIT_REVISION = 194; + public static final String GIT_SHA = "ccc02c62e0f2bb7ae54a362b635224abbe9098ef"; + public static final String GIT_DATE = "2026-03-17 17:06:49 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-17 17:05:46 CDT"; - public static final long BUILD_UNIX_TIME = 1773785146530L; + public static final String BUILD_DATE = "2026-03-17 17:18:59 CDT"; + public static final long BUILD_UNIX_TIME = 1773785939903L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java index b866ac10..9619b486 100644 --- a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java +++ b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.mechanisms.feeder.FeederState; import frc.robot.subsystems.mechanisms.feeder.FeederSubsystem; +import frc.robot.subsystems.mechanisms.intake.IntakeConstants; import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; @@ -38,7 +39,7 @@ public void registerCommands(){ NamedCommands.registerCommand("IntakeDown", Commands.runOnce( () -> { - intakeSubsystem.setPosition(0.0); + intakeSubsystem.setPosition(IntakeConstants.minValue); } ) ); @@ -47,7 +48,7 @@ public void registerCommands(){ NamedCommands.registerCommand("IntakeUp", Commands.runOnce( () -> { - intakeSubsystem.setPosition(-6.0); + intakeSubsystem.setPosition(IntakeConstants.maxValue); } ) ); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java index e7631c8b..0e7688d8 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java @@ -46,6 +46,7 @@ public FeederSubsystem() { // MARK: Periodic Loop @Override public void periodic() { + logValues(); runBedAgitation(); switch (feederState) { diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java index fdd680e5..41d0a4f9 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java @@ -2,6 +2,6 @@ public class IntakeConstants { public static final double minValue = 0.0; - public static final double maxValue = 7.0; - public static final double threshold = 0.5; + public static final double maxValue = 2.34; + public static final double threshold = 0.4; } From b4fe74009b891eefbd5ce6d5aa0c0bced3beebb8 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 18:06:27 -0500 Subject: [PATCH 071/102] fixes to intake --- .../mechanisms/intake/IntakeSubsystem.java | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 78198130..d058b8dd 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -10,18 +10,19 @@ public class IntakeSubsystem extends SubsystemBase { // MARK: Intake Angle public Motor angleMotor = new Motor(9, "Mechanisms") - .setFree(false); - - // MARK: Intake Wheels - public Motor intakeWheelsMotor = new Motor(10, "Mechanisms") + .setFree(false) .setMinValue(IntakeConstants.minValue) .setMaxValue(IntakeConstants.maxValue) - .setMotorSpeed(0.2) + .setMotorSpeed(0.3) .setHoldSpeed(0.0) .setThreshold(IntakeConstants.threshold) - .setMinSpeed(0.1) - .setPG(0.1) - .setPositionSupplier(()-> angleMotor.getCurrentValue()); + .setMinSpeed(0.05) + .setPG(0.1); + + // MARK: Intake Wheels + public Motor intakeWheelsMotor = new Motor(10, "Mechanisms") + .setFree(true) + .setMotorSpeed(0.2); public IntakeSubsystem() { angleMotor.toggleEnabled(true); @@ -39,7 +40,7 @@ public void periodic() { public void setPosition(double targetPosition) { Logger.recordOutput("IntakeSubsystem/SetPosition", targetPosition); - intakeWheelsMotor.goTo(targetPosition); + angleMotor.goTo(targetPosition); } private double intakeSpeed = 0.2; From ff3618ff55a933049f62d4a367fe602013f39a97 Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Tue, 17 Mar 2026 18:18:05 -0500 Subject: [PATCH 072/102] luyhresres --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/joysticks/DebugJoystick.java | 6 +----- .../subsystems/mechanisms/intake/IntakeSubsystem.java | 3 ++- 3 files changed, 8 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 92d5f33b..fc82087b 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 194; - public static final String GIT_SHA = "ccc02c62e0f2bb7ae54a362b635224abbe9098ef"; - public static final String GIT_DATE = "2026-03-17 17:06:49 CDT"; + public static final int GIT_REVISION = 196; + public static final String GIT_SHA = "b4fe74009b891eefbd5ce6d5aa0c0bced3beebb8"; + public static final String GIT_DATE = "2026-03-17 18:06:27 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-17 17:18:59 CDT"; - public static final long BUILD_UNIX_TIME = 1773785939903L; + public static final String BUILD_DATE = "2026-03-17 18:17:01 CDT"; + public static final long BUILD_UNIX_TIME = 1773789421700L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 3f97ca4e..af152ab3 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -83,11 +83,7 @@ public void configureBindings() { ); joystick.b().onTrue( - Commands.runOnce( - () -> { - feederSubsystem.setFeederState(FeederState.ALL_FEEDER_OUT); - } - ) + NamedCommands.getCommand("IntakeUp") ); joystick.x().onTrue( diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index d058b8dd..ebabb8b3 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -13,7 +13,7 @@ public class IntakeSubsystem extends SubsystemBase { .setFree(false) .setMinValue(IntakeConstants.minValue) .setMaxValue(IntakeConstants.maxValue) - .setMotorSpeed(0.3) + .setMotorSpeed(1.0) .setHoldSpeed(0.0) .setThreshold(IntakeConstants.threshold) .setMinSpeed(0.05) @@ -36,6 +36,7 @@ public IntakeSubsystem() { @Override public void periodic() { Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getCurrentValue()); + Logger.recordOutput("IntakeSubsystem/IntakeAngleSpeed", angleMotor.getMotor().get()); } public void setPosition(double targetPosition) { From 0da33fc80f62f5e1761350a462210c31b259a0f0 Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Tue, 17 Mar 2026 18:21:14 -0500 Subject: [PATCH 073/102] asdfsadfsfdasadf --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- .../subsystems/mechanisms/intake/IntakeSubsystem.java | 6 +++--- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index fc82087b..9590a8ba 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 196; - public static final String GIT_SHA = "b4fe74009b891eefbd5ce6d5aa0c0bced3beebb8"; - public static final String GIT_DATE = "2026-03-17 18:06:27 CDT"; + public static final int GIT_REVISION = 197; + public static final String GIT_SHA = "ff3618ff55a933049f62d4a367fe602013f39a97"; + public static final String GIT_DATE = "2026-03-17 18:18:05 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-17 18:17:01 CDT"; - public static final long BUILD_UNIX_TIME = 1773789421700L; + public static final String BUILD_DATE = "2026-03-17 18:20:20 CDT"; + public static final long BUILD_UNIX_TIME = 1773789620161L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index ebabb8b3..c151059e 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -13,11 +13,11 @@ public class IntakeSubsystem extends SubsystemBase { .setFree(false) .setMinValue(IntakeConstants.minValue) .setMaxValue(IntakeConstants.maxValue) - .setMotorSpeed(1.0) + .setMotorSpeed(0.8) .setHoldSpeed(0.0) .setThreshold(IntakeConstants.threshold) - .setMinSpeed(0.05) - .setPG(0.1); + .setMinSpeed(0.3) + .setPG(0.5); // MARK: Intake Wheels public Motor intakeWheelsMotor = new Motor(10, "Mechanisms") From a6ad7d7bd303f28a68f02c6db98ef06c9a912f8e Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 18:29:20 -0500 Subject: [PATCH 074/102] asdfasdfasdfasdf --- .../robot/subsystems/mechanisms/intake/IntakeSubsystem.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index c151059e..e86f1412 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -17,7 +17,7 @@ public class IntakeSubsystem extends SubsystemBase { .setHoldSpeed(0.0) .setThreshold(IntakeConstants.threshold) .setMinSpeed(0.3) - .setPG(0.5); + .setPG(0.8); // MARK: Intake Wheels public Motor intakeWheelsMotor = new Motor(10, "Mechanisms") @@ -25,6 +25,8 @@ public class IntakeSubsystem extends SubsystemBase { .setMotorSpeed(0.2); public IntakeSubsystem() { + angleMotor.setPositionSupplier(() -> angleMotor.getMotor().getPosition().refresh().getValueAsDouble()); + angleMotor.toggleEnabled(true); intakeWheelsMotor.toggleEnabled(true); From a9635fc67d6d94dac8e12a19518b4203009b99ee Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 18:42:36 -0500 Subject: [PATCH 075/102] did stuff --- src/main/java/frc/robot/joysticks/DebugJoystick.java | 4 +--- .../subsystems/mechanisms/intake/IntakeSubsystem.java | 11 +++++++---- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index af152ab3..b9379e3a 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -69,7 +69,7 @@ public DebugJoystick( Commands.run( () -> { double triggerSpeed = joystick.getLeftTriggerAxis() > joystick.getRightTriggerAxis() ? -joystick.getLeftTriggerAxis(): joystick.getRightTriggerAxis(); - intakeSubsystem.intakeWheelsMotor.drive(triggerSpeed); + intakeSubsystem.intakeWheelsMotor.getMotor().set(triggerSpeed); Logger.recordOutput("IntakeSubsystem/WheelSpeed", triggerSpeed); }, intakeSubsystem @@ -119,8 +119,6 @@ public void configureBindings() { ) ); - joystick.rightBumper(); - joystick.leftBumper(); // Reset pose to limelight output diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index e86f1412..b95d531a 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -4,6 +4,7 @@ import com.btwrobotics.WhatTime.frc.MotorManagers.Motor; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotContainer; @@ -17,7 +18,7 @@ public class IntakeSubsystem extends SubsystemBase { .setHoldSpeed(0.0) .setThreshold(IntakeConstants.threshold) .setMinSpeed(0.3) - .setPG(0.8); + .setPG(1.0); // MARK: Intake Wheels public Motor intakeWheelsMotor = new Motor(10, "Mechanisms") @@ -29,6 +30,8 @@ public IntakeSubsystem() { angleMotor.toggleEnabled(true); intakeWheelsMotor.toggleEnabled(true); + + intakeWheelsMotor.setDefaultCommand(Commands.run(() -> {}, intakeWheelsMotor)); angleMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); intakeWheelsMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); @@ -52,13 +55,13 @@ public void setIntake(IntakeStates intakeState) { Logger.recordOutput("IntakeSubsystem/State", intakeState.toString()); switch (intakeState) { case INTAKE_IN: - intakeWheelsMotor.drive(intakeSpeed); + intakeWheelsMotor.getMotor().set(intakeSpeed); break; case INTAKE_OUT: - intakeWheelsMotor.drive(-intakeSpeed); + intakeWheelsMotor.getMotor().set(-intakeSpeed); break; case OFF: - intakeWheelsMotor.drive(0); + intakeWheelsMotor.getMotor().set(0.0); break; default: break; From 605f81c3a0f4101ecc50d1b627a0ff4a41649eb8 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 18:51:07 -0500 Subject: [PATCH 076/102] more things --- src/main/java/frc/robot/BuildConstants.java | 12 +++++------ .../frc/robot/joysticks/DebugJoystick.java | 20 +++++++++---------- vendordeps/WhatTime.json | 4 ++-- 3 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 9590a8ba..3d9a4e07 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,14 +5,14 @@ */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "roomba"; + public static final String MAVEN_NAME = "2026 Roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 197; - public static final String GIT_SHA = "ff3618ff55a933049f62d4a367fe602013f39a97"; - public static final String GIT_DATE = "2026-03-17 18:18:05 CDT"; + public static final int GIT_REVISION = 200; + public static final String GIT_SHA = "a9635fc67d6d94dac8e12a19518b4203009b99ee"; + public static final String GIT_DATE = "2026-03-17 18:42:36 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-17 18:20:20 CDT"; - public static final long BUILD_UNIX_TIME = 1773789620161L; + public static final String BUILD_DATE = "2026-03-17 18:50:51 CDT"; + public static final long BUILD_UNIX_TIME = 1773791451137L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index b9379e3a..5701433b 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -65,16 +65,16 @@ public DebugJoystick( ) ); - intakeSubsystem.setDefaultCommand( - Commands.run( - () -> { - double triggerSpeed = joystick.getLeftTriggerAxis() > joystick.getRightTriggerAxis() ? -joystick.getLeftTriggerAxis(): joystick.getRightTriggerAxis(); - intakeSubsystem.intakeWheelsMotor.getMotor().set(triggerSpeed); - - Logger.recordOutput("IntakeSubsystem/WheelSpeed", triggerSpeed); - }, intakeSubsystem - ) - ); + // intakeSubsystem.setDefaultCommand( + // Commands.run( + // () -> { + // double triggerSpeed = joystick.getLeftTriggerAxis() > joystick.getRightTriggerAxis() ? -joystick.getLeftTriggerAxis(): joystick.getRightTriggerAxis(); + // intakeSubsystem.intakeWheelsMotor.getMotor().set(triggerSpeed); + + // Logger.recordOutput("IntakeSubsystem/WheelSpeed", triggerSpeed); + // }, intakeSubsystem + // ) + // ); } public void configureBindings() { diff --git a/vendordeps/WhatTime.json b/vendordeps/WhatTime.json index 5b349c6f..ebdd6a9c 100644 --- a/vendordeps/WhatTime.json +++ b/vendordeps/WhatTime.json @@ -1,7 +1,7 @@ { "fileName": "WhatTime.json", "name": "WhatTime", - "version": "v2026.2.1-beta.2", + "version": "v2026.2.1-beta.3", "frcYear": 2026, "uuid": "2C0B26E9-47AA-4C55-B235-640C45ECAD33", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.btwrobotics.whattime.frc", "artifactId": "whattime", - "version": "v2026.2.1-beta.2" + "version": "v2026.2.1-beta.3" } ], "jniDependencies": [], From f254272aef0cf7ee416835cc431e2acc4f2689cb Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 18:55:40 -0500 Subject: [PATCH 077/102] asdfasdfasdfdasf --- .../mechanisms/intake/IntakeSubsystem.java | 45 ++++++++++++------- 1 file changed, 30 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index b95d531a..60b020ba 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -10,29 +10,24 @@ public class IntakeSubsystem extends SubsystemBase { // MARK: Intake Angle - public Motor angleMotor = new Motor(9, "Mechanisms") - .setFree(false) - .setMinValue(IntakeConstants.minValue) - .setMaxValue(IntakeConstants.maxValue) - .setMotorSpeed(0.8) - .setHoldSpeed(0.0) - .setThreshold(IntakeConstants.threshold) - .setMinSpeed(0.3) - .setPG(1.0); + public Motor angleMotor = new Motor(9, "Mechanisms"); // MARK: Intake Wheels public Motor intakeWheelsMotor = new Motor(10, "Mechanisms") .setFree(true) .setMotorSpeed(0.2); - public IntakeSubsystem() { - angleMotor.setPositionSupplier(() -> angleMotor.getMotor().getPosition().refresh().getValueAsDouble()); + // MARK: Angle Control State + private double angleTarget = Double.NaN; + private static final double ANGLE_SPEED = 0.3; + public IntakeSubsystem() { angleMotor.toggleEnabled(true); intakeWheelsMotor.toggleEnabled(true); + angleMotor.setDefaultCommand(Commands.run(() -> {}, angleMotor)); intakeWheelsMotor.setDefaultCommand(Commands.run(() -> {}, intakeWheelsMotor)); - + angleMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); intakeWheelsMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); } @@ -40,13 +35,33 @@ public IntakeSubsystem() { // MARK: Periodic Loop @Override public void periodic() { - Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getCurrentValue()); - Logger.recordOutput("IntakeSubsystem/IntakeAngleSpeed", angleMotor.getMotor().get()); + runAngleControl(); + + Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getMotor().getPosition().refresh().getValueAsDouble()); + Logger.recordOutput("IntakeSubsystem/AngleTarget", Double.isNaN(angleTarget) ? -1.0 : angleTarget); + Logger.recordOutput("IntakeSubsystem/AngleSpeed", angleMotor.getMotor().get()); + } + + // MARK: Angle Control + private void runAngleControl() { + if (Double.isNaN(angleTarget)) { + angleMotor.getMotor().set(0.0); + return; + } + + double currentPos = angleMotor.getMotor().getPosition().refresh().getValueAsDouble(); + double error = angleTarget - currentPos; + + if (Math.abs(error) <= IntakeConstants.threshold) { + angleMotor.getMotor().set(0.0); + } else { + angleMotor.getMotor().set(Math.copySign(ANGLE_SPEED, error)); + } } public void setPosition(double targetPosition) { Logger.recordOutput("IntakeSubsystem/SetPosition", targetPosition); - angleMotor.goTo(targetPosition); + angleTarget = targetPosition; } private double intakeSpeed = 0.2; From 246fbb359703224727bb426401974d854445ebe4 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 19:07:14 -0500 Subject: [PATCH 078/102] changes to intake stuff --- .../frc/robot/joysticks/DebugJoystick.java | 21 ++++++++++--------- .../mechanisms/intake/IntakeSubsystem.java | 2 +- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 5701433b..e5fce3ee 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -102,20 +102,21 @@ public void configureBindings() { ) ); - joystick.rightTrigger().whileTrue( - NamedCommands.getCommand("RunAllFeederIn") - ); + joystick.rightTrigger() + .onTrue( + Commands.runOnce( + () -> { + intakeSubsystem.setIntake(IntakeStates.OFF); + } + ) + ); joystick.leftTrigger() - .whileTrue( - Commands.runEnd( + .onTrue( + Commands.runOnce( () -> { intakeSubsystem.setIntake(IntakeStates.INTAKE_IN); - }, - () -> { - intakeSubsystem.setIntake(IntakeStates.OFF); - }, - intakeSubsystem + } ) ); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 60b020ba..211bb249 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -19,7 +19,7 @@ public class IntakeSubsystem extends SubsystemBase { // MARK: Angle Control State private double angleTarget = Double.NaN; - private static final double ANGLE_SPEED = 0.3; + private static final double ANGLE_SPEED = 0.6; public IntakeSubsystem() { angleMotor.toggleEnabled(true); From ce6d6a2bf0d183a99f1e21439a317a71cf4a89a6 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 19:09:33 -0500 Subject: [PATCH 079/102] changed angle intake to talonfx --- src/main/java/frc/robot/Robot.java | 4 ++-- .../mechanisms/intake/IntakeSubsystem.java | 22 ++++++++++--------- 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4f014f59..cb73c705 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -95,7 +95,7 @@ public void robotInit() { robotContainer.shooterSubsystem.shooterPitchMotor.toggleEnabled(true); robotContainer.shooterSubsystem.leftShooterMotor.toggleEnabled(true); robotContainer.shooterSubsystem.rightShooterMotor.toggleEnabled(true); - robotContainer.intakeSubsystem.angleMotor.toggleEnabled(true); + // robotContainer.intakeSubsystem.angleMotor.toggleEnabled(true); robotContainer.intakeSubsystem.intakeWheelsMotor.toggleEnabled(true); robotContainer.feederSubsystem.feederBedMotor.toggleEnabled(true); robotContainer.feederSubsystem.feederFeederMotor.toggleEnabled(true); @@ -106,7 +106,7 @@ public void robotInit() { Logger.recordOutput("MotorConfig/ShooterPitch", robotContainer.shooterSubsystem.shooterPitchMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); Logger.recordOutput("MotorConfig/LeftShooter", robotContainer.shooterSubsystem.leftShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); Logger.recordOutput("MotorConfig/RightShooter", robotContainer.shooterSubsystem.rightShooterMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); - Logger.recordOutput("MotorConfig/IntakeAngle", robotContainer.intakeSubsystem.angleMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); + // Logger.recordOutput("MotorConfig/IntakeAngle", robotContainer.intakeSubsystem.angleMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); Logger.recordOutput("MotorConfig/IntakeWheels", robotContainer.intakeSubsystem.intakeWheelsMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); Logger.recordOutput("MotorConfig/FeederBed", robotContainer.feederSubsystem.feederBedMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); Logger.recordOutput("MotorConfig/FeederFeeder", robotContainer.feederSubsystem.feederFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration).toString()); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 211bb249..1458b466 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -3,6 +3,7 @@ import org.littletonrobotics.junction.Logger; import com.btwrobotics.WhatTime.frc.MotorManagers.Motor; +import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -10,7 +11,8 @@ public class IntakeSubsystem extends SubsystemBase { // MARK: Intake Angle - public Motor angleMotor = new Motor(9, "Mechanisms"); + // public Motor angleMotor = new Motor(9, "Mechanisms"); + public TalonFX angleMotor = new TalonFX(9, "Mechanisms"); // MARK: Intake Wheels public Motor intakeWheelsMotor = new Motor(10, "Mechanisms") @@ -22,13 +24,13 @@ public class IntakeSubsystem extends SubsystemBase { private static final double ANGLE_SPEED = 0.6; public IntakeSubsystem() { - angleMotor.toggleEnabled(true); + // angleMotor.toggleEnabled(true); intakeWheelsMotor.toggleEnabled(true); - angleMotor.setDefaultCommand(Commands.run(() -> {}, angleMotor)); + // angleMotor.setDefaultCommand(Commands.run(() -> {}, angleMotor)); intakeWheelsMotor.setDefaultCommand(Commands.run(() -> {}, intakeWheelsMotor)); - angleMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + angleMotor.getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); intakeWheelsMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); } @@ -37,25 +39,25 @@ public IntakeSubsystem() { public void periodic() { runAngleControl(); - Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getMotor().getPosition().refresh().getValueAsDouble()); + Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getPosition().refresh().getValueAsDouble()); Logger.recordOutput("IntakeSubsystem/AngleTarget", Double.isNaN(angleTarget) ? -1.0 : angleTarget); - Logger.recordOutput("IntakeSubsystem/AngleSpeed", angleMotor.getMotor().get()); + Logger.recordOutput("IntakeSubsystem/AngleSpeed", angleMotor.get()); } // MARK: Angle Control private void runAngleControl() { if (Double.isNaN(angleTarget)) { - angleMotor.getMotor().set(0.0); + angleMotor.set(0.0); return; } - double currentPos = angleMotor.getMotor().getPosition().refresh().getValueAsDouble(); + double currentPos = angleMotor.getPosition().refresh().getValueAsDouble(); double error = angleTarget - currentPos; if (Math.abs(error) <= IntakeConstants.threshold) { - angleMotor.getMotor().set(0.0); + angleMotor.set(0.0); } else { - angleMotor.getMotor().set(Math.copySign(ANGLE_SPEED, error)); + angleMotor.set(Math.copySign(ANGLE_SPEED, error)); } } From c8a5dddd4a1edcc26c7b56a758dedd345ed79bdd Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 19:23:27 -0500 Subject: [PATCH 080/102] asdfasdfasdfdfsa --- .../frc/robot/joysticks/DebugJoystick.java | 19 +++++-------------- 1 file changed, 5 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index e5fce3ee..1a9b02b1 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -102,21 +102,12 @@ public void configureBindings() { ) ); - joystick.rightTrigger() - .onTrue( - Commands.runOnce( - () -> { - intakeSubsystem.setIntake(IntakeStates.OFF); - } - ) - ); - joystick.leftTrigger() - .onTrue( - Commands.runOnce( - () -> { - intakeSubsystem.setIntake(IntakeStates.INTAKE_IN); - } + .whileTrue( + Commands.runEnd( + () -> intakeSubsystem.setIntake(IntakeStates.INTAKE_IN), + () -> intakeSubsystem.setIntake(IntakeStates.OFF), + intakeSubsystem ) ); From 5f1628487bdcee391a3c7c4e22117df7224775eb Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Tue, 17 Mar 2026 19:27:56 -0500 Subject: [PATCH 081/102] changes to intake wheels --- .../mechanisms/intake/IntakeSubsystem.java | 35 +++++++++++-------- 1 file changed, 21 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 1458b466..a8b6d4fe 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -15,9 +15,7 @@ public class IntakeSubsystem extends SubsystemBase { public TalonFX angleMotor = new TalonFX(9, "Mechanisms"); // MARK: Intake Wheels - public Motor intakeWheelsMotor = new Motor(10, "Mechanisms") - .setFree(true) - .setMotorSpeed(0.2); + public Motor intakeWheelsMotor = new Motor(10, "Mechanisms"); // MARK: Angle Control State private double angleTarget = Double.NaN; @@ -34,14 +32,20 @@ public IntakeSubsystem() { intakeWheelsMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); } + // MARK: Intake Wheel State + private IntakeStates intakeState = IntakeStates.OFF; + private static final double INTAKE_SPEED = 0.2; + // MARK: Periodic Loop @Override public void periodic() { runAngleControl(); + runIntakeWheels(); Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getPosition().refresh().getValueAsDouble()); Logger.recordOutput("IntakeSubsystem/AngleTarget", Double.isNaN(angleTarget) ? -1.0 : angleTarget); Logger.recordOutput("IntakeSubsystem/AngleSpeed", angleMotor.get()); + Logger.recordOutput("IntakeSubsystem/WheelState", intakeState.toString()); } // MARK: Angle Control @@ -61,27 +65,30 @@ private void runAngleControl() { } } - public void setPosition(double targetPosition) { - Logger.recordOutput("IntakeSubsystem/SetPosition", targetPosition); - angleTarget = targetPosition; - } - - private double intakeSpeed = 0.2; - - public void setIntake(IntakeStates intakeState) { - Logger.recordOutput("IntakeSubsystem/State", intakeState.toString()); + // MARK: Intake Wheels + private void runIntakeWheels() { switch (intakeState) { case INTAKE_IN: - intakeWheelsMotor.getMotor().set(intakeSpeed); + intakeWheelsMotor.getMotor().set(INTAKE_SPEED); break; case INTAKE_OUT: - intakeWheelsMotor.getMotor().set(-intakeSpeed); + intakeWheelsMotor.getMotor().set(-INTAKE_SPEED); break; case OFF: intakeWheelsMotor.getMotor().set(0.0); break; default: + intakeWheelsMotor.getMotor().set(0.0); break; } } + + public void setPosition(double targetPosition) { + Logger.recordOutput("IntakeSubsystem/SetPosition", targetPosition); + angleTarget = targetPosition; + } + + public void setIntake(IntakeStates intakeState) { + this.intakeState = intakeState; + } } From a578c97c73b57fab13fece21d6badf6a858db993 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Wed, 18 Mar 2026 10:11:57 -0500 Subject: [PATCH 082/102] did things --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/joysticks/OperatorJoystick.java | 14 ++++++++------ .../mechanisms/shooter/ShooterSubsystem.java | 5 ++++- 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6b23146d..48a0cc3e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -61,7 +61,7 @@ public class RobotContainer { public static final TalonFXConfiguration mechanismsMotorConfiguration = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() - .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimit(Amps.of(100)) .withSupplyCurrentLimit(Amps.of(40)) .withStatorCurrentLimitEnable(true) .withSupplyCurrentLimitEnable(true) diff --git a/src/main/java/frc/robot/joysticks/OperatorJoystick.java b/src/main/java/frc/robot/joysticks/OperatorJoystick.java index d0125ea4..6cb576b8 100644 --- a/src/main/java/frc/robot/joysticks/OperatorJoystick.java +++ b/src/main/java/frc/robot/joysticks/OperatorJoystick.java @@ -37,11 +37,7 @@ public void configureBindings() { NamedCommands.getCommand("IntakeDown") ); - // MARK: Intake Up - joystick.b().onTrue( - NamedCommands.getCommand("IntakeUp") - ); - + joystick.b(); joystick.x(); @@ -51,7 +47,13 @@ public void configureBindings() { joystick.rightTrigger(); // MARK: Intake in - joystick.leftTrigger(); + joystick.leftTrigger().whileTrue( + Commands.runEnd( + () -> intakeSubsystem.setIntake(IntakeStates.INTAKE_IN), + () -> intakeSubsystem.setIntake(IntakeStates.OFF), + intakeSubsystem + ) + ); // MARK: Shooter feeder joystick.rightBumper(); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 8081aeb4..77355bb3 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -47,7 +47,9 @@ public class ShooterSubsystem extends SubsystemBase { public final Motor shooterPitchMotor = new Motor(11, "Mechanisms", true) .setFree(false) .setRange(ShooterConstants.SHOOTER_MIN_ANGLE, ShooterConstants.SHOOTER_MAX_ANGLE) - .setMotorSpeed(0.1) + .setMotorSpeed(0.5) + .setMinSpeed(0.05) + .setPG(0.02) .setThreshold(ShooterConstants.POSITION_THRESHOLD) .setPositionSupplier(() -> getPigeonPosition()); @@ -88,6 +90,7 @@ public void periodic() { Logger.recordOutput("ShooterSubsystem/MotorConnected", shooterPitchMotor.getMotor().isConnected()); Logger.recordOutput("ShooterSubsystem/ThroughBoreAngle", getThroughBorePosition()); Logger.recordOutput("ShooterSubsystem/PigeonAngle", getPigeonPosition()); + Logger.recordOutput("ShooterSubsystem/PitchMotorOutput", shooterPitchMotor.getMotor().get()); } // --- Commands --- \\ From 18569ab3b5ec8907b8cf0580740aecbc15dacfe1 Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Wed, 18 Mar 2026 10:26:31 -0500 Subject: [PATCH 083/102] Got intake up and down working. --- src/main/java/frc/robot/BuildConstants.java | 12 ++++++------ .../java/frc/robot/joysticks/OperatorJoystick.java | 14 +++++++------- .../mechanisms/intake/IntakeSubsystem.java | 10 +++++++--- 3 files changed, 20 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 3d9a4e07..8894d399 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,14 +5,14 @@ */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026 Roomba"; + public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 200; - public static final String GIT_SHA = "a9635fc67d6d94dac8e12a19518b4203009b99ee"; - public static final String GIT_DATE = "2026-03-17 18:42:36 CDT"; + public static final int GIT_REVISION = 207; + public static final String GIT_SHA = "a578c97c73b57fab13fece21d6badf6a858db993"; + public static final String GIT_DATE = "2026-03-18 10:11:57 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-17 18:50:51 CDT"; - public static final long BUILD_UNIX_TIME = 1773791451137L; + public static final String BUILD_DATE = "2026-03-18 10:22:30 CDT"; + public static final long BUILD_UNIX_TIME = 1773847350136L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/joysticks/OperatorJoystick.java b/src/main/java/frc/robot/joysticks/OperatorJoystick.java index 6cb576b8..25c147aa 100644 --- a/src/main/java/frc/robot/joysticks/OperatorJoystick.java +++ b/src/main/java/frc/robot/joysticks/OperatorJoystick.java @@ -47,13 +47,13 @@ public void configureBindings() { joystick.rightTrigger(); // MARK: Intake in - joystick.leftTrigger().whileTrue( - Commands.runEnd( - () -> intakeSubsystem.setIntake(IntakeStates.INTAKE_IN), - () -> intakeSubsystem.setIntake(IntakeStates.OFF), - intakeSubsystem - ) - ); + // joystick.leftTrigger().whileTrue( + // Commands.runEnd( + // () -> intakeSubsystem.setIntake(IntakeStates.INTAKE_IN), + // () -> intakeSubsystem.setIntake(IntakeStates.OFF), + // intakeSubsystem + // ) + // ); // MARK: Shooter feeder joystick.rightBumper(); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index a8b6d4fe..14aae870 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -19,7 +19,8 @@ public class IntakeSubsystem extends SubsystemBase { // MARK: Angle Control State private double angleTarget = Double.NaN; - private static final double ANGLE_SPEED = 0.6; + private static final double ANGLE_DOWN_SPEED = 0.2; + private static final double ANGLE_UP_SPEED = 0.2; public IntakeSubsystem() { // angleMotor.toggleEnabled(true); @@ -60,8 +61,11 @@ private void runAngleControl() { if (Math.abs(error) <= IntakeConstants.threshold) { angleMotor.set(0.0); - } else { - angleMotor.set(Math.copySign(ANGLE_SPEED, error)); + } else if (error < 0) { + angleMotor.set(Math.copySign(ANGLE_DOWN_SPEED, error)); + } + else { + angleMotor.set(Math.copySign(ANGLE_UP_SPEED, error)); } } From ff42fc24b66bef7986809416429dfb2ee2c06b6c Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Wed, 18 Mar 2026 10:34:05 -0500 Subject: [PATCH 084/102] things --- src/main/java/frc/robot/joysticks/DebugJoystick.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 1a9b02b1..1dfcfb50 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -111,7 +111,13 @@ public void configureBindings() { ) ); - joystick.leftBumper(); + joystick.leftBumper().onTrue( + Commands.runOnce( + () -> { + shooterSubsystem.shooterPitchMotor.set(50); + } + ) + ); // Reset pose to limelight output joystick.povUp().onTrue( From 38b7aad2921b8859a2c655301b0b886ef0962e33 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Wed, 18 Mar 2026 11:46:45 -0500 Subject: [PATCH 085/102] Removed bed agitation. More adjustments --- .../mechanisms/feeder/FeederSubsystem.java | 42 ++++++------------- .../mechanisms/intake/IntakeSubsystem.java | 2 +- .../mechanisms/shooter/ShooterSubsystem.java | 3 +- 3 files changed, 16 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java index 0e7688d8..ae223324 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java @@ -3,7 +3,6 @@ import org.littletonrobotics.junction.Logger; import com.btwrobotics.WhatTime.frc.MotorManagers.Motor; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotContainer; @@ -22,11 +21,6 @@ public class FeederSubsystem extends SubsystemBase { // MARK: Feeder State private FeederState feederState = FeederState.OFF; - // MARK: Bed Agitation Timer - private final Timer bedAgitationTimer = new Timer(); - private static final double BED_FORWARD_SECONDS = 10.0; - private static final double BED_REVERSE_SECONDS = 2.0; - // MARK: Constructor public FeederSubsystem() { feederBedMotor.toggleEnabled(true); @@ -37,72 +31,62 @@ public FeederSubsystem() { feederFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); shooterFeederMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + feederBedMotor.setDefaultCommand(Commands.run(() -> {}, feederBedMotor)); feederFeederMotor.setDefaultCommand(Commands.run(() -> {}, feederFeederMotor)); shooterFeederMotor.setDefaultCommand(Commands.run(() -> {}, shooterFeederMotor)); - - bedAgitationTimer.start(); } // MARK: Periodic Loop @Override public void periodic() { logValues(); - runBedAgitation(); switch (feederState) { case ALL_FEEDER_IN: - runSpecifiedMotors(true, false, true, false); + runSpecifiedMotors(true, false, true, false, true, false); break; case ALL_FEEDER_OUT: - runSpecifiedMotors(true, true, true, true); + runSpecifiedMotors(true, true, true, true, true, true); break; case SHOOTER_FEED_IN: - runSpecifiedMotors(true, false, true, false); + runSpecifiedMotors(true, false, true, false, false, false); break; case SHOOTER_FEED_OUT: - runSpecifiedMotors(true, true, true, true); + runSpecifiedMotors(true, true, true, true, false, false); break; case OFF: - runSpecifiedMotors(false, false, false, false); + runSpecifiedMotors(false, false, false, false, false, false); break; default: - runSpecifiedMotors(false, false, false, false); + runSpecifiedMotors(false, false, false, false, false, false); break; } } - // MARK: Bed Agitation - // Drives forward for 10 seconds, reverses for 2 seconds, repeats. - private void runBedAgitation() { - double t = bedAgitationTimer.get() % (BED_FORWARD_SECONDS + BED_REVERSE_SECONDS); - if (t < BED_FORWARD_SECONDS) { - feederBedMotor.drive(); - } else { - feederBedMotor.drive(true); - } - - Logger.recordOutput("FeederSubsystem/BedAgitationTime", t); - } - private void runSpecifiedMotors( boolean runFeederFeeder, boolean feederFeederInverted, boolean runShooterFeeder, - boolean shooterFeederInverted + boolean shooterFeederInverted, + boolean runBed, + boolean bedInverted ) { double feederSpeed = runFeederFeeder ? (feederFeederInverted ? -FeederConstants.FEEDER_FEEDER_SPEED : FeederConstants.FEEDER_FEEDER_SPEED) : 0.0; double shooterSpeed = runShooterFeeder ? (shooterFeederInverted ? -FeederConstants.SHOOTER_FEEDER_SPEED : FeederConstants.SHOOTER_FEEDER_SPEED) : 0.0; + double bedSpeed = runBed ? (bedInverted ? -FeederConstants.FEEDER_BED_SPEED : FeederConstants.FEEDER_BED_SPEED) : 0.0; feederFeederMotor.getMotor().set(feederSpeed); shooterFeederMotor.getMotor().set(shooterSpeed); + feederBedMotor.getMotor().set(bedSpeed); Logger.recordOutput("FeederSubsystem/FeederFeederSpeed", feederSpeed); Logger.recordOutput("FeederSubsystem/ShooterFeederSpeed", shooterSpeed); + Logger.recordOutput("FeederSubsystem/BedSpeed", bedSpeed); Logger.recordOutput("FeederSubsystem/FeederState", feederState.toString()); } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 14aae870..223652b9 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -35,7 +35,7 @@ public IntakeSubsystem() { // MARK: Intake Wheel State private IntakeStates intakeState = IntakeStates.OFF; - private static final double INTAKE_SPEED = 0.2; + private static final double INTAKE_SPEED = 0.5; // MARK: Periodic Loop @Override diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 77355bb3..44e6be4d 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -48,7 +48,8 @@ public class ShooterSubsystem extends SubsystemBase { .setFree(false) .setRange(ShooterConstants.SHOOTER_MIN_ANGLE, ShooterConstants.SHOOTER_MAX_ANGLE) .setMotorSpeed(0.5) - .setMinSpeed(0.05) + .setMinSpeed(0.1) + .setHoldSpeed(0.1) .setPG(0.02) .setThreshold(ShooterConstants.POSITION_THRESHOLD) .setPositionSupplier(() -> getPigeonPosition()); From 3c8ccf0aea9ee1a39d453cf0894d68f7beff5854 Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Wed, 18 Mar 2026 11:57:34 -0500 Subject: [PATCH 086/102] changes --- src/main/java/frc/robot/BuildConstants.java | 12 ++++++------ .../commands/namedcommands/RegisterCommands.java | 9 +++++++++ 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 8894d399..97ce3740 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,13 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 207; - public static final String GIT_SHA = "a578c97c73b57fab13fece21d6badf6a858db993"; - public static final String GIT_DATE = "2026-03-18 10:11:57 CDT"; + public static final int GIT_REVISION = 210; + public static final String GIT_SHA = "38b7aad2921b8859a2c655301b0b886ef0962e33"; + public static final String GIT_DATE = "2026-03-18 11:46:45 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-18 10:22:30 CDT"; - public static final long BUILD_UNIX_TIME = 1773847350136L; - public static final int DIRTY = 1; + public static final String BUILD_DATE = "2026-03-18 11:47:40 CDT"; + public static final long BUILD_UNIX_TIME = 1773852460364L; + public static final int DIRTY = 0; private BuildConstants(){} } diff --git a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java index 9619b486..64763fc0 100644 --- a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java +++ b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java @@ -53,6 +53,15 @@ public void registerCommands(){ ) ); + // MARK: IntakeAgitate + NamedCommands.registerCommand("IntakeAgitate", + Commands.runOnce( + () -> { + intakeSubsystem.setPosition(IntakeConstants.maxValue/2); + } + ) + ); + // MARK: RunIntakeIn NamedCommands.registerCommand("IntakeIn", Commands.runOnce( From 6c9d9acd7d86b11c14ebe6d33e889b259c1138d1 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Thu, 19 Mar 2026 10:47:52 -0500 Subject: [PATCH 087/102] Updated autonomous routine for neutral zone. Removed old autos. Added more named commands. Moved intake angle motor back to WhatTime motor. --- src/main/deploy/pathplanner/autos/1- HOH.auto | 31 ----- .../deploy/pathplanner/autos/2 - HDH.auto | 31 ----- .../deploy/pathplanner/autos/3- HDHOH.auto | 31 ----- .../deploy/pathplanner/autos/4 - HNH.auto | 25 ---- .../pathplanner/autos/Neutral Auto 1.auto | 62 ++++++++++ .../pathplanner/paths/HDH - Part 2.path | 54 --------- .../pathplanner/paths/HDH - Part 3.path | 54 --------- .../pathplanner/paths/HDH2 - Part 2.path | 54 --------- .../pathplanner/paths/HDH2 - Part 3.path | 75 ------------ .../pathplanner/paths/HDHOH - Part 1.path | 86 -------------- .../pathplanner/paths/HDHOH - Part 2.path | 97 --------------- .../pathplanner/paths/HDHOH - Part 3.path | 65 ---------- .../pathplanner/paths/HNH - Part 1.path | 6 +- .../pathplanner/paths/HNH - Part 2.path | 8 +- .../pathplanner/paths/HdH2 - Part 1.path | 65 ---------- ...rt 1.path => Neutral Auto 1 - Part 1.path} | 30 ++--- .../paths/Neutral Auto 1 - Part 2.path | 111 ++++++++++++++++++ .../paths/Neutral Auto 1 - Part 3.path | 111 ++++++++++++++++++ .../pathplanner/paths/Testing Pose Path.path | 8 +- src/main/deploy/pathplanner/settings.json | 20 ++-- src/main/java/frc/robot/BuildConstants.java | 14 +-- .../namedcommands/RegisterCommands.java | 16 ++- .../frc/robot/joysticks/DebugJoystick.java | 1 - .../mechanisms/intake/IntakeConstants.java | 10 +- .../mechanisms/intake/IntakeSubsystem.java | 25 ++-- .../mechanisms/shooter/ShooterConstants.java | 28 +++-- .../vision/limelight/LimelightConstants.java | 2 +- 27 files changed, 374 insertions(+), 746 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/1- HOH.auto delete mode 100644 src/main/deploy/pathplanner/autos/2 - HDH.auto delete mode 100644 src/main/deploy/pathplanner/autos/3- HDHOH.auto delete mode 100644 src/main/deploy/pathplanner/autos/4 - HNH.auto create mode 100644 src/main/deploy/pathplanner/autos/Neutral Auto 1.auto delete mode 100644 src/main/deploy/pathplanner/paths/HDH - Part 2.path delete mode 100644 src/main/deploy/pathplanner/paths/HDH - Part 3.path delete mode 100644 src/main/deploy/pathplanner/paths/HDH2 - Part 2.path delete mode 100644 src/main/deploy/pathplanner/paths/HDH2 - Part 3.path delete mode 100644 src/main/deploy/pathplanner/paths/HDHOH - Part 1.path delete mode 100644 src/main/deploy/pathplanner/paths/HDHOH - Part 2.path delete mode 100644 src/main/deploy/pathplanner/paths/HDHOH - Part 3.path delete mode 100644 src/main/deploy/pathplanner/paths/HdH2 - Part 1.path rename src/main/deploy/pathplanner/paths/{HDH - Part 1.path => Neutral Auto 1 - Part 1.path} (59%) create mode 100644 src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 2.path create mode 100644 src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 3.path diff --git a/src/main/deploy/pathplanner/autos/1- HOH.auto b/src/main/deploy/pathplanner/autos/1- HOH.auto deleted file mode 100644 index bd0da273..00000000 --- a/src/main/deploy/pathplanner/autos/1- HOH.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "HdH2 - Part 1" - } - }, - { - "type": "path", - "data": { - "pathName": "HDH2 - Part 2" - } - }, - { - "type": "path", - "data": { - "pathName": "HDH2 - Part 3" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 - HDH.auto b/src/main/deploy/pathplanner/autos/2 - HDH.auto deleted file mode 100644 index 92b1a6de..00000000 --- a/src/main/deploy/pathplanner/autos/2 - HDH.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "HDH - Part 1" - } - }, - { - "type": "path", - "data": { - "pathName": "HDH - Part 2" - } - }, - { - "type": "path", - "data": { - "pathName": "HDH - Part 3" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3- HDHOH.auto b/src/main/deploy/pathplanner/autos/3- HDHOH.auto deleted file mode 100644 index c6b0ddff..00000000 --- a/src/main/deploy/pathplanner/autos/3- HDHOH.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "HDHOH - Part 1" - } - }, - { - "type": "path", - "data": { - "pathName": "HDHOH - Part 2" - } - }, - { - "type": "path", - "data": { - "pathName": "HDHOH - Part 3" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4 - HNH.auto b/src/main/deploy/pathplanner/autos/4 - HNH.auto deleted file mode 100644 index 7a0f4614..00000000 --- a/src/main/deploy/pathplanner/autos/4 - HNH.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "HNH - Part 1" - } - }, - { - "type": "path", - "data": { - "pathName": "HNH - Part 2" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto b/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto new file mode 100644 index 00000000..fa3e6916 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto @@ -0,0 +1,62 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "IntakeDown" + } + }, + { + "type": "path", + "data": { + "pathName": "Neutral Auto 1 - Part 1" + } + }, + { + "type": "path", + "data": { + "pathName": "Neutral Auto 1 - Part 2" + } + }, + { + "type": "path", + "data": { + "pathName": "Neutral Auto 1 - Part 3" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShootAutoFuel" + } + }, + { + "type": "named", + "data": { + "name": "AgitateAutoFuel" + } + }, + { + "type": "named", + "data": { + "name": "RunAllFeederIn" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 2.path b/src/main/deploy/pathplanner/paths/HDH - Part 2.path deleted file mode 100644 index b0ad5da3..00000000 --- a/src/main/deploy/pathplanner/paths/HDH - Part 2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.4183219762731487, - "y": 4.7485145399305555 - }, - "prevControl": null, - "nextControl": { - "x": 0.42060589674932697, - "y": 5.224863656477267 - }, - "isLocked": false, - "linkedName": "HDH - 1" - }, - { - "anchor": { - "x": 0.4183219762731487, - "y": 6.927243489583334 - }, - "prevControl": { - "x": 0.42114279554876993, - "y": 7.177227575034016 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "HDH - 2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -89.62181552849039 - }, - "reversed": false, - "folder": "1 - Hub, Depot, Hub (HDH)", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 3.path b/src/main/deploy/pathplanner/paths/HDH - Part 3.path deleted file mode 100644 index 6388e837..00000000 --- a/src/main/deploy/pathplanner/paths/HDH - Part 3.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.4183219762731487, - "y": 6.927243489583334 - }, - "prevControl": null, - "nextControl": { - "x": 2.097426721643519, - "y": 6.502415292245371 - }, - "isLocked": false, - "linkedName": "HDH - 2" - }, - { - "anchor": { - "x": 3.6354688223379634, - "y": 4.255910662615741 - }, - "prevControl": { - "x": 2.313219762731482, - "y": 4.215888237847222 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "1 - Hub, Depot, Hub (HDH)", - "idealStartingState": { - "velocity": 0, - "rotation": -89.62181552849039 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDH2 - Part 2.path b/src/main/deploy/pathplanner/paths/HDH2 - Part 2.path deleted file mode 100644 index c094d63a..00000000 --- a/src/main/deploy/pathplanner/paths/HDH2 - Part 2.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.3739620403321475, - "y": 5.277686832740213 - }, - "prevControl": null, - "nextControl": { - "x": 1.5670225385527887, - "y": 6.364365361803084 - }, - "isLocked": false, - "linkedName": "HOH - 1" - }, - { - "anchor": { - "x": 0.38351126927639445, - "y": 7.2358600237247925 - }, - "prevControl": { - "x": 1.355656635563977, - "y": 7.2358600237247925 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "HOH - 2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "2- HDH", - "idealStartingState": { - "velocity": 0, - "rotation": -35.059426966886996 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDH2 - Part 3.path b/src/main/deploy/pathplanner/paths/HDH2 - Part 3.path deleted file mode 100644 index b7b2c7f6..00000000 --- a/src/main/deploy/pathplanner/paths/HDH2 - Part 3.path +++ /dev/null @@ -1,75 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.38351126927639445, - "y": 7.2358600237247925 - }, - "prevControl": null, - "nextControl": { - "x": 0.3835112692763945, - "y": 6.590308422301304 - }, - "isLocked": false, - "linkedName": "HOH - 2" - }, - { - "anchor": { - "x": 0.38351126927639445, - "y": 5.23465005931198 - }, - "prevControl": { - "x": 0.3835112692763944, - "y": 6.052419535551767 - }, - "nextControl": { - "x": 0.3835112692763945, - "y": 4.380533311470473 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.643546856465007, - "y": 4.266322657176749 - }, - "prevControl": { - "x": 2.3094068801898002, - "y": 5.977034400948991 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.077127659574468, - "rotationDegrees": 90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "2- HDH", - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path b/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path deleted file mode 100644 index 6c42786c..00000000 --- a/src/main/deploy/pathplanner/paths/HDHOH - Part 1.path +++ /dev/null @@ -1,86 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6901465947210452, - "y": 6.353015205685028 - }, - "prevControl": null, - "nextControl": { - "x": 1.9768597288320737, - "y": 5.994681095862107 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.5577825410487289, - "y": 5.175910961776131 - }, - "prevControl": { - "x": 0.8534255274540963, - "y": 4.392190920727401 - }, - "nextControl": { - "x": 0.4462496135096857, - "y": 5.471573617643542 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.3952998653778249, - "y": 6.7493993864759885 - }, - "prevControl": { - "x": 0.39529986537782497, - "y": 5.589795892453111 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "HDHOH - 1" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.9785940803382666, - "rotationDegrees": -78.6156485289686 - } - ], - "constraintZones": [], - "pointTowardsZones": [ - { - "fieldPosition": { - "x": 4.626, - "y": 4.034 - }, - "rotationOffset": 0.0, - "minWaypointRelativePos": 0.1, - "maxWaypointRelativePos": 0.7675682106598984, - "name": "Point Towards Zone" - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -90.0 - }, - "reversed": false, - "folder": "3 - Hub, Depot, Hub, Outpost, Hub (HDHOH)", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path b/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path deleted file mode 100644 index 384be26b..00000000 --- a/src/main/deploy/pathplanner/paths/HDHOH - Part 2.path +++ /dev/null @@ -1,97 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.3952998653778249, - "y": 6.7493993864759885 - }, - "prevControl": null, - "nextControl": { - "x": 1.5322086464191353, - "y": 7.019345013370341 - }, - "isLocked": false, - "linkedName": "HDHOH - 1" - }, - { - "anchor": { - "x": 2.06691660045904, - "y": 4.663688923463983 - }, - "prevControl": { - "x": 2.013865287368215, - "y": 5.407969223567639 - }, - "nextControl": { - "x": 2.114577540165961, - "y": 3.995032552083333 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.5759363413665257, - "y": 2.3252545131532485 - }, - "prevControl": { - "x": 1.6788570280910506, - "y": 2.589121599919883 - }, - "nextControl": { - "x": 1.4843147081995867, - "y": 2.0903558334226107 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.3866386387711864, - "y": 0.703621281338277 - }, - "prevControl": { - "x": 0.6692790215537343, - "y": 1.7680030816735008 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "HDHOH - 2" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [ - { - "fieldPosition": { - "x": 4.626, - "y": 4.034 - }, - "rotationOffset": 0.0, - "minWaypointRelativePos": 0.6, - "maxWaypointRelativePos": 2.2081218274111687, - "name": "Point Towards Zone" - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 88.67957408826696 - }, - "reversed": false, - "folder": "3 - Hub, Depot, Hub, Outpost, Hub (HDHOH)", - "idealStartingState": { - "velocity": 0, - "rotation": -90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDHOH - Part 3.path b/src/main/deploy/pathplanner/paths/HDHOH - Part 3.path deleted file mode 100644 index 866e6d25..00000000 --- a/src/main/deploy/pathplanner/paths/HDHOH - Part 3.path +++ /dev/null @@ -1,65 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 0.3866386387711864, - "y": 0.703621281338277 - }, - "prevControl": null, - "nextControl": { - "x": 0.930785889823216, - "y": 1.477687215900453 - }, - "isLocked": false, - "linkedName": "HDHOH - 2" - }, - { - "anchor": { - "x": 2.0376910090042375, - "y": 3.9501296566031074 - }, - "prevControl": { - "x": 2.2442816429688675, - "y": 3.028219417632113 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [ - { - "fieldPosition": { - "x": 4.626, - "y": 4.034 - }, - "rotationOffset": 0.0, - "minWaypointRelativePos": 0.25, - "maxWaypointRelativePos": 1.0, - "name": "Point Towards Zone" - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": "3 - Hub, Depot, Hub, Outpost, Hub (HDHOH)", - "idealStartingState": { - "velocity": 0, - "rotation": 88.67957408826696 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HNH - Part 1.path b/src/main/deploy/pathplanner/paths/HNH - Part 1.path index 59d4944d..c1ea00d4 100644 --- a/src/main/deploy/pathplanner/paths/HNH - Part 1.path +++ b/src/main/deploy/pathplanner/paths/HNH - Part 1.path @@ -34,9 +34,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 100.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/HNH - Part 2.path b/src/main/deploy/pathplanner/paths/HNH - Part 2.path index 57dea564..1147b109 100644 --- a/src/main/deploy/pathplanner/paths/HNH - Part 2.path +++ b/src/main/deploy/pathplanner/paths/HNH - Part 2.path @@ -115,15 +115,15 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 100.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -40.601294645004465 + "rotation": -40.60129464500447 }, "reversed": false, "folder": "4 - Hub, Neutral, Hub (HNH)", diff --git a/src/main/deploy/pathplanner/paths/HdH2 - Part 1.path b/src/main/deploy/pathplanner/paths/HdH2 - Part 1.path deleted file mode 100644 index 9349af2f..00000000 --- a/src/main/deploy/pathplanner/paths/HdH2 - Part 1.path +++ /dev/null @@ -1,65 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6543060498220648, - "y": 4.997947805456701 - }, - "prevControl": null, - "nextControl": { - "x": 2.7699915705321745, - "y": 5.235037351543269 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.3739620403321475, - "y": 5.277686832740213 - }, - "prevControl": { - "x": 3.09472357017964, - "y": 4.973614579936882 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "HOH - 1" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [ - { - "fieldPosition": { - "x": 4.626, - "y": 4.034 - }, - "rotationOffset": 0.0, - "minWaypointRelativePos": 0.17521704814522504, - "maxWaypointRelativePos": 1.0, - "name": "Point Towards Zone" - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 100.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -35.059426966886996 - }, - "reversed": false, - "folder": "2- HDH", - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HDH - Part 1.path b/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 1.path similarity index 59% rename from src/main/deploy/pathplanner/paths/HDH - Part 1.path rename to src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 1.path index efb2675b..8a1facd6 100644 --- a/src/main/deploy/pathplanner/paths/HDH - Part 1.path +++ b/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 1.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 3.6576451822916667, - "y": 4.265358579282408 + "x": 3.6423778464146634, + "y": 6.404003254994157 }, "prevControl": null, "nextControl": { - "x": 3.1337778093985262, - "y": 4.522571785294896 + "x": 2.861505823087231, + "y": 6.123399727941238 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 0.4183219762731487, - "y": 4.7485145399305555 + "x": 2.8143530092596523, + "y": 5.99025947626901 }, "prevControl": { - "x": 1.957807508680556, - "y": 5.192435402199075 + "x": 3.053391608063426, + "y": 6.063475241524374 }, "nextControl": null, "isLocked": false, - "linkedName": "HDH - 1" + "linkedName": "Neutral Auto 1 Shoot Point" } ], "rotationTargets": [], @@ -34,21 +34,21 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 100.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -90.0 + "rotation": -47.94801452480744 }, "reversed": false, - "folder": "1 - Hub, Depot, Hub (HDH)", + "folder": "Neutral Auto 1", "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 2.path b/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 2.path new file mode 100644 index 00000000..3d336079 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 2.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.8143530092596523, + "y": 5.99025947626901 + }, + "prevControl": null, + "nextControl": { + "x": 3.02515964988426, + "y": 7.242436487268519 + }, + "isLocked": false, + "linkedName": "Neutral Auto 1 Shoot Point" + }, + { + "anchor": { + "x": 4.620807798032408, + "y": 7.4582951388888885 + }, + "prevControl": { + "x": 3.9797272858796298, + "y": 7.508552806712963 + }, + "nextControl": { + "x": 5.170787300200828, + "y": 7.415179357632004 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.312873553240742, + "y": 6.518686704282407 + }, + "prevControl": { + "x": 7.115889970256496, + "y": 7.695417124580676 + }, + "nextControl": { + "x": 7.5003228443287036, + "y": 5.398911747685186 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.538508174189815, + "y": 2.361668981481482 + }, + "prevControl": { + "x": 7.428807364004628, + "y": 4.075809751161541 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Neutral Auto 1 - Point 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.187386924342105, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.8302837171052628, + "rotationDegrees": 121.2021594294515 + }, + { + "waypointRelativePos": 2.207411595394738, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeIn", + "waypointRelativePos": 1.404614825581395, + "endWaypointRelativePos": 3.0, + "command": { + "type": "named", + "data": { + "name": "IntakeIn" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Neutral Auto 1", + "idealStartingState": { + "velocity": 0, + "rotation": -47.94801452480744 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 3.path b/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 3.path new file mode 100644 index 00000000..ff705257 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 3.path @@ -0,0 +1,111 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.538508174189815, + "y": 2.361668981481482 + }, + "prevControl": null, + "nextControl": { + "x": 7.53332494212963, + "y": 3.2277280092592595 + }, + "isLocked": false, + "linkedName": "Neutral Auto 1 - Point 2" + }, + { + "anchor": { + "x": 6.976160300925926, + "y": 7.062269965277778 + }, + "prevControl": { + "x": 7.595831920744345, + "y": 6.5210564522765715 + }, + "nextControl": { + "x": 6.3442311660349535, + "y": 7.614189040032846 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.121577256944445, + "y": 7.533025535300926 + }, + "prevControl": { + "x": 4.972873914930556, + "y": 7.6331472077546305 + }, + "nextControl": { + "x": 3.559445142355874, + "y": 7.4669127332306235 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8143530092596523, + "y": 5.99025947626901 + }, + "prevControl": { + "x": 2.9189795103551757, + "y": 6.217312983770014 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Neutral Auto 1 Shoot Point" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.25494449013157894, + "rotationDegrees": -95.2427421246706 + }, + { + "waypointRelativePos": 1.3980160361842107, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 2.061173930921052, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeIn", + "waypointRelativePos": 0, + "endWaypointRelativePos": 1.645880917774087, + "command": { + "type": "named", + "data": { + "name": "IntakeIn" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -47.94801452480744 + }, + "reversed": false, + "folder": "Neutral Auto 1", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Testing Pose Path.path b/src/main/deploy/pathplanner/paths/Testing Pose Path.path index 16cf14ff..c82bea20 100644 --- a/src/main/deploy/pathplanner/paths/Testing Pose Path.path +++ b/src/main/deploy/pathplanner/paths/Testing Pose Path.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 3.393546856465007, - "y": 0.3392170818505338 + "y": 0.33921708185053384 }, "isLocked": false, "linkedName": null @@ -34,9 +34,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 180.0, - "maxAngularAcceleration": 100.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 240de202..001e6042 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,18 +1,16 @@ { - "robotWidth": 0.762, - "robotLength": 0.762, + "robotWidth": 0.7874, + "robotLength": 0.7874, "holonomicMode": true, "pathFolders": [ - "1 - Hub, Depot, Hub (HDH)", - "2- HDH", - "3 - Hub, Depot, Hub, Outpost, Hub (HDHOH)", - "4 - Hub, Neutral, Hub (HNH)" + "4 - Hub, Neutral, Hub (HNH)", + "Neutral Auto 1" ], "autoFolders": [], "defaultMaxVel": 3.0, - "defaultMaxAccel": 1.5, - "defaultMaxAngVel": 180.0, - "defaultMaxAngAccel": 100.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, "robotMass": 75.0, "robotMOI": 6.883, @@ -33,5 +31,7 @@ "brModuleY": -0.267, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, - "robotFeatures": [] + "robotFeatures": [ + "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.425,\"y\":0.0},\"size\":{\"width\":0.6,\"length\":0.15},\"borderRadius\":0.02,\"strokeWidth\":0.01,\"filled\":false}}" + ] } \ No newline at end of file diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 97ce3740..8bd58fe9 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,15 +5,15 @@ */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "roomba"; + public static final String MAVEN_NAME = "2026 Roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 210; - public static final String GIT_SHA = "38b7aad2921b8859a2c655301b0b886ef0962e33"; - public static final String GIT_DATE = "2026-03-18 11:46:45 CDT"; + public static final int GIT_REVISION = 211; + public static final String GIT_SHA = "3c8ccf0aea9ee1a39d453cf0894d68f7beff5854"; + public static final String GIT_DATE = "2026-03-18 11:57:34 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-18 11:47:40 CDT"; - public static final long BUILD_UNIX_TIME = 1773852460364L; - public static final int DIRTY = 0; + public static final String BUILD_DATE = "2026-03-19 10:37:48 CDT"; + public static final long BUILD_UNIX_TIME = 1773934668869L; + public static final int DIRTY = 1; private BuildConstants(){} } diff --git a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java index 64763fc0..911341ef 100644 --- a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java +++ b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java @@ -39,7 +39,7 @@ public void registerCommands(){ NamedCommands.registerCommand("IntakeDown", Commands.runOnce( () -> { - intakeSubsystem.setPosition(IntakeConstants.minValue); + intakeSubsystem.setPosition(IntakeConstants.INTAKE_MIN_VALUE); } ) ); @@ -48,7 +48,7 @@ public void registerCommands(){ NamedCommands.registerCommand("IntakeUp", Commands.runOnce( () -> { - intakeSubsystem.setPosition(IntakeConstants.maxValue); + intakeSubsystem.setPosition(IntakeConstants.INTAKE_MAX_VALUE); } ) ); @@ -57,11 +57,21 @@ public void registerCommands(){ NamedCommands.registerCommand("IntakeAgitate", Commands.runOnce( () -> { - intakeSubsystem.setPosition(IntakeConstants.maxValue/2); + intakeSubsystem.setPosition(IntakeConstants.INTAKE_MAX_VALUE/2); } ) ); + // MARK: AgitateAutoFuel + NamedCommands.registerCommand("AgitateAutoFuel", + Commands.repeatingSequence( + Commands.runOnce(() -> intakeSubsystem.setPosition(IntakeConstants.INTAKE_MAX_VALUE / 2)), + Commands.waitSeconds(2), + Commands.runOnce(() -> intakeSubsystem.setPosition(IntakeConstants.INTAKE_MIN_VALUE)), + Commands.waitSeconds(2) + ) + ); + // MARK: RunIntakeIn NamedCommands.registerCommand("IntakeIn", Commands.runOnce( diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 1dfcfb50..b68d8130 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java index 41d0a4f9..8b50d183 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java @@ -1,7 +1,11 @@ package frc.robot.subsystems.mechanisms.intake; public class IntakeConstants { - public static final double minValue = 0.0; - public static final double maxValue = 2.34; - public static final double threshold = 0.4; + public static final double INTAKE_MIN_VALUE = 0.0; + public static final double INTAKE_MAX_VALUE = 2.34; + public static final double INTAKE_THRESHOLD = 0.4; + + public static final double INTAKE_WHEELS_SPEED = 0.5; + public static final double INTAKE_DOWN_SPEED = 0.2; + public static final double INTAKE_UP_SPEED = 0.2; } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 223652b9..7d05f774 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -11,16 +11,14 @@ public class IntakeSubsystem extends SubsystemBase { // MARK: Intake Angle - // public Motor angleMotor = new Motor(9, "Mechanisms"); - public TalonFX angleMotor = new TalonFX(9, "Mechanisms"); + public Motor angleMotor = new Motor(9, "Mechanisms"); + // public TalonFX angleMotor = new TalonFX(9, "Mechanisms"); // MARK: Intake Wheels public Motor intakeWheelsMotor = new Motor(10, "Mechanisms"); // MARK: Angle Control State private double angleTarget = Double.NaN; - private static final double ANGLE_DOWN_SPEED = 0.2; - private static final double ANGLE_UP_SPEED = 0.2; public IntakeSubsystem() { // angleMotor.toggleEnabled(true); @@ -29,13 +27,12 @@ public IntakeSubsystem() { // angleMotor.setDefaultCommand(Commands.run(() -> {}, angleMotor)); intakeWheelsMotor.setDefaultCommand(Commands.run(() -> {}, intakeWheelsMotor)); - angleMotor.getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); + angleMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); intakeWheelsMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); } // MARK: Intake Wheel State private IntakeStates intakeState = IntakeStates.OFF; - private static final double INTAKE_SPEED = 0.5; // MARK: Periodic Loop @Override @@ -43,9 +40,9 @@ public void periodic() { runAngleControl(); runIntakeWheels(); - Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getPosition().refresh().getValueAsDouble()); + Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getMotor().getPosition().refresh().getValueAsDouble()); Logger.recordOutput("IntakeSubsystem/AngleTarget", Double.isNaN(angleTarget) ? -1.0 : angleTarget); - Logger.recordOutput("IntakeSubsystem/AngleSpeed", angleMotor.get()); + Logger.recordOutput("IntakeSubsystem/AngleSpeed", angleMotor.getMotor().get()); Logger.recordOutput("IntakeSubsystem/WheelState", intakeState.toString()); } @@ -56,16 +53,16 @@ private void runAngleControl() { return; } - double currentPos = angleMotor.getPosition().refresh().getValueAsDouble(); + double currentPos = angleMotor.getMotor().getPosition().refresh().getValueAsDouble(); double error = angleTarget - currentPos; - if (Math.abs(error) <= IntakeConstants.threshold) { + if (Math.abs(error) <= IntakeConstants.INTAKE_THRESHOLD) { angleMotor.set(0.0); } else if (error < 0) { - angleMotor.set(Math.copySign(ANGLE_DOWN_SPEED, error)); + angleMotor.set(Math.copySign(IntakeConstants.INTAKE_DOWN_SPEED, error)); } else { - angleMotor.set(Math.copySign(ANGLE_UP_SPEED, error)); + angleMotor.set(Math.copySign(IntakeConstants.INTAKE_UP_SPEED, error)); } } @@ -73,10 +70,10 @@ private void runAngleControl() { private void runIntakeWheels() { switch (intakeState) { case INTAKE_IN: - intakeWheelsMotor.getMotor().set(INTAKE_SPEED); + intakeWheelsMotor.getMotor().set(IntakeConstants.INTAKE_WHEELS_SPEED); break; case INTAKE_OUT: - intakeWheelsMotor.getMotor().set(-INTAKE_SPEED); + intakeWheelsMotor.getMotor().set(-IntakeConstants.INTAKE_WHEELS_SPEED); break; case OFF: intakeWheelsMotor.getMotor().set(0.0); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java index a4ceeb5b..18fd42e7 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java @@ -5,19 +5,21 @@ public class ShooterConstants { public static final List shooterDataPoints = List.of( // TODO: Collect successful data points and add them here - new ShooterDataPoint(0.0, 0, 0), - new ShooterDataPoint(0.5, 0, 0), - new ShooterDataPoint(1.0, 0, 0), - new ShooterDataPoint(1.5, 0, 0), - new ShooterDataPoint(2.0, 0, 0), - new ShooterDataPoint(2.5, 0, 0), - new ShooterDataPoint(3.0, 0, 0), - new ShooterDataPoint(3.5, 0, 0), - new ShooterDataPoint(4.0, 0, 0), - new ShooterDataPoint(4.5, 0, 0), - new ShooterDataPoint(5.0, 0, 0), - new ShooterDataPoint(5.5, 0, 0), - new ShooterDataPoint(6.0, 0, 0) + // new ShooterDataPoint(0.0, 0, 0), + // new ShooterDataPoint(0.5, 0, 0), + // new ShooterDataPoint(1.0, 0, 0), + // new ShooterDataPoint(1.5, 0, 0), + // new ShooterDataPoint(2.0, 0, 0), + // new ShooterDataPoint(2.5, 0, 0), + // new ShooterDataPoint(3.0, 0, 0), + // new ShooterDataPoint(3.5, 0, 0), + // new ShooterDataPoint(4.0, 0, 0), + // new ShooterDataPoint(4.5, 0, 0), + // new ShooterDataPoint(5.0, 0, 0), + // new ShooterDataPoint(5.5, 0, 0), + // new ShooterDataPoint(6.0, 0, 0) + new ShooterDataPoint(1.4986, 69.6, 0.65), + new ShooterDataPoint(2.775, 45.5, 0.74) ); // --- Shooter configuration and tuning fields --- diff --git a/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java b/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java index 65bfa796..bfebbee7 100644 --- a/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/limelight/LimelightConstants.java @@ -26,7 +26,7 @@ public class LimelightConstants { // Position of Limelight relative to the centre of the robot in metres public static final Transform2d LIMELIGHT_4_TRANSFORM_FROM_CENTRE = new Transform2d( new Translation2d(0.0762, -0.26035), - new Rotation2d(-0.2792526803) + new Rotation2d(-0.174532925199433) ); /** Calculate dynamic standard deviations for Quest */ From 9c5976945831a7f6a4b3dfc6fc28601ab43c1151 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Thu, 19 Mar 2026 11:30:22 -0500 Subject: [PATCH 088/102] More path updates. Started working on controllers. --- .../pathplanner/autos/Neutral Auto 1.auto | 2 +- .../pathplanner/autos/Neutral Auto 2.auto | 12 ++ .../pathplanner/paths/HNH - Part 2.path | 135 ------------------ .../paths/Neutral Auto 1 - Part 1.path | 49 +++++-- .../paths/Neutral Auto 1 - Part 2.path | 111 -------------- .../paths/Neutral Auto 1 - Part 3.path | 111 -------------- ...rt 1.path => Neutral Auto 2 - Part 1.path} | 24 ++-- .../paths/Neutral Auto 2 - Part 2.path | 79 ++++++++++ src/main/deploy/pathplanner/settings.json | 4 +- src/main/java/frc/robot/Robot.java | 1 - .../frc/robot/joysticks/OperatorJoystick.java | 43 ++++-- .../mechanisms/intake/IntakeSubsystem.java | 9 ++ 12 files changed, 182 insertions(+), 398 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Neutral Auto 2.auto delete mode 100644 src/main/deploy/pathplanner/paths/HNH - Part 2.path delete mode 100644 src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 2.path delete mode 100644 src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 3.path rename src/main/deploy/pathplanner/paths/{HNH - Part 1.path => Neutral Auto 2 - Part 1.path} (66%) create mode 100644 src/main/deploy/pathplanner/paths/Neutral Auto 2 - Part 2.path diff --git a/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto b/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto index fa3e6916..4b173f77 100644 --- a/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto +++ b/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto @@ -19,7 +19,7 @@ { "type": "path", "data": { - "pathName": "Neutral Auto 1 - Part 2" + "pathName": "Old Neutral Auto 1 - Part 2" } }, { diff --git a/src/main/deploy/pathplanner/autos/Neutral Auto 2.auto b/src/main/deploy/pathplanner/autos/Neutral Auto 2.auto new file mode 100644 index 00000000..440a1ea5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Neutral Auto 2.auto @@ -0,0 +1,12 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HNH - Part 2.path b/src/main/deploy/pathplanner/paths/HNH - Part 2.path deleted file mode 100644 index 1147b109..00000000 --- a/src/main/deploy/pathplanner/paths/HNH - Part 2.path +++ /dev/null @@ -1,135 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.9902241327342325, - "y": 5.418686721030622 - }, - "prevControl": null, - "nextControl": { - "x": 3.9902241327342325, - "y": 5.418686721030622 - }, - "isLocked": false, - "linkedName": "HNH - 1" - }, - { - "anchor": { - "x": 4.801654177873641, - "y": 5.418686721030622 - }, - "prevControl": { - "x": 4.395939155303937, - "y": 5.418686721030622 - }, - "nextControl": { - "x": 5.207369200443345, - "y": 5.418686721030622 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.613689205219455, - "y": 7.160545670225385 - }, - "prevControl": { - "x": 7.463060498220641, - "y": 7.666227758007118 - }, - "nextControl": { - "x": 7.721191628014499, - "y": 6.79964467941345 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.463060498220641, - "y": 4.804282325029655 - }, - "prevControl": { - "x": 7.674464556831574, - "y": 5.170716026621937 - }, - "nextControl": { - "x": 7.301672597864768, - "y": 4.524543297746145 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.806144721233689, - "y": 5.632740213523132 - }, - "prevControl": { - "x": 6.444918747871875, - "y": 5.564822805456703 - }, - "nextControl": { - "x": 5.167370694595504, - "y": 5.700657621589561 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.3960854092526698, - "y": 5.084021352313167 - }, - "prevControl": { - "x": 4.073914590747332, - "y": 5.600462633451957 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.7499999999999987, - "rotationDegrees": -45.0 - }, - { - "waypointRelativePos": 2.05, - "rotationDegrees": 90.0 - }, - { - "waypointRelativePos": 2.932180851063824, - "rotationDegrees": 90.0 - }, - { - "waypointRelativePos": 4.035904255319144, - "rotationDegrees": -45.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -40.60129464500447 - }, - "reversed": false, - "folder": "4 - Hub, Neutral, Hub (HNH)", - "idealStartingState": { - "velocity": 0, - "rotation": -40.38523342211157 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 1.path b/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 1.path index 8a1facd6..757a5f23 100644 --- a/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 1.path +++ b/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 1.path @@ -3,32 +3,57 @@ "waypoints": [ { "anchor": { - "x": 3.6423778464146634, - "y": 6.404003254994157 + "x": 4.455928530092593, + "y": 7.19860865162037 }, "prevControl": null, "nextControl": { - "x": 2.861505823087231, - "y": 6.123399727941238 + "x": 4.793412840783701, + "y": 7.126602132123037 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.8143530092596523, - "y": 5.99025947626901 + "x": 7.160853949652778, + "y": 7.029202256944444 }, "prevControl": { - "x": 3.053391608063426, - "y": 6.063475241524374 + "x": 6.6386748822686785, + "y": 7.591245387179791 + }, + "nextControl": { + "x": 7.80660430720271, + "y": 6.334154188230901 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.538508174189815, + "y": 2.361668981481482 + }, + "prevControl": { + "x": 7.389464795846691, + "y": 3.8473959916429026 }, "nextControl": null, "isLocked": false, - "linkedName": "Neutral Auto 1 Shoot Point" + "linkedName": "Neutral Auto 1 - Point 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1729029605263157, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 1.0609580592105259, + "rotationDegrees": 90.0 } ], - "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], @@ -42,13 +67,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -47.94801452480744 + "rotation": 90.0 }, "reversed": false, "folder": "Neutral Auto 1", "idealStartingState": { "velocity": 0, - "rotation": -90.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 2.path b/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 2.path deleted file mode 100644 index 3d336079..00000000 --- a/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 2.path +++ /dev/null @@ -1,111 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.8143530092596523, - "y": 5.99025947626901 - }, - "prevControl": null, - "nextControl": { - "x": 3.02515964988426, - "y": 7.242436487268519 - }, - "isLocked": false, - "linkedName": "Neutral Auto 1 Shoot Point" - }, - { - "anchor": { - "x": 4.620807798032408, - "y": 7.4582951388888885 - }, - "prevControl": { - "x": 3.9797272858796298, - "y": 7.508552806712963 - }, - "nextControl": { - "x": 5.170787300200828, - "y": 7.415179357632004 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.312873553240742, - "y": 6.518686704282407 - }, - "prevControl": { - "x": 7.115889970256496, - "y": 7.695417124580676 - }, - "nextControl": { - "x": 7.5003228443287036, - "y": 5.398911747685186 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.538508174189815, - "y": 2.361668981481482 - }, - "prevControl": { - "x": 7.428807364004628, - "y": 4.075809751161541 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Neutral Auto 1 - Point 2" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.187386924342105, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 1.8302837171052628, - "rotationDegrees": 121.2021594294515 - }, - { - "waypointRelativePos": 2.207411595394738, - "rotationDegrees": 90.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "IntakeIn", - "waypointRelativePos": 1.404614825581395, - "endWaypointRelativePos": 3.0, - "command": { - "type": "named", - "data": { - "name": "IntakeIn" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": "Neutral Auto 1", - "idealStartingState": { - "velocity": 0, - "rotation": -47.94801452480744 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 3.path b/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 3.path deleted file mode 100644 index ff705257..00000000 --- a/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 3.path +++ /dev/null @@ -1,111 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.538508174189815, - "y": 2.361668981481482 - }, - "prevControl": null, - "nextControl": { - "x": 7.53332494212963, - "y": 3.2277280092592595 - }, - "isLocked": false, - "linkedName": "Neutral Auto 1 - Point 2" - }, - { - "anchor": { - "x": 6.976160300925926, - "y": 7.062269965277778 - }, - "prevControl": { - "x": 7.595831920744345, - "y": 6.5210564522765715 - }, - "nextControl": { - "x": 6.3442311660349535, - "y": 7.614189040032846 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.121577256944445, - "y": 7.533025535300926 - }, - "prevControl": { - "x": 4.972873914930556, - "y": 7.6331472077546305 - }, - "nextControl": { - "x": 3.559445142355874, - "y": 7.4669127332306235 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.8143530092596523, - "y": 5.99025947626901 - }, - "prevControl": { - "x": 2.9189795103551757, - "y": 6.217312983770014 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Neutral Auto 1 Shoot Point" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.25494449013157894, - "rotationDegrees": -95.2427421246706 - }, - { - "waypointRelativePos": 1.3980160361842107, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 2.061173930921052, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "IntakeIn", - "waypointRelativePos": 0, - "endWaypointRelativePos": 1.645880917774087, - "command": { - "type": "named", - "data": { - "name": "IntakeIn" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -47.94801452480744 - }, - "reversed": false, - "folder": "Neutral Auto 1", - "idealStartingState": { - "velocity": 0, - "rotation": 90.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HNH - Part 1.path b/src/main/deploy/pathplanner/paths/Neutral Auto 2 - Part 1.path similarity index 66% rename from src/main/deploy/pathplanner/paths/HNH - Part 1.path rename to src/main/deploy/pathplanner/paths/Neutral Auto 2 - Part 1.path index c1ea00d4..5fdaa0f4 100644 --- a/src/main/deploy/pathplanner/paths/HNH - Part 1.path +++ b/src/main/deploy/pathplanner/paths/Neutral Auto 2 - Part 1.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 3.641908216307055, - "y": 6.084090469520768 + "x": 3.61808203125, + "y": 3.84925658275463 }, "prevControl": null, "nextControl": { - "x": 3.095264899171878, - "y": 5.963307755446868 + "x": 3.36808203125, + "y": 3.84925658275463 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.9902241327342325, - "y": 5.418686721030622 + "x": 2.89557884837963, + "y": 3.84925658275463 }, "prevControl": { - "x": 3.2445456114622395, - "y": 5.617218969128363 + "x": 3.14557884837963, + "y": 3.84925658275463 }, "nextControl": null, "isLocked": false, - "linkedName": "HNH - 1" + "linkedName": "Neutral Auto 2 - Shoot Point" } ], "rotationTargets": [], @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -40.38523342211157 + "rotation": 0.0 }, "reversed": false, - "folder": "4 - Hub, Neutral, Hub (HNH)", + "folder": "Neutral Auto 2", "idealStartingState": { "velocity": 0, - "rotation": -90.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Neutral Auto 2 - Part 2.path b/src/main/deploy/pathplanner/paths/Neutral Auto 2 - Part 2.path new file mode 100644 index 00000000..832f518a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Neutral Auto 2 - Part 2.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.89557884837963, + "y": 3.84925658275463 + }, + "prevControl": null, + "nextControl": { + "x": 2.83915379050926, + "y": 2.2673210358796303 + }, + "isLocked": false, + "linkedName": "Neutral Auto 2 - Shoot Point" + }, + { + "anchor": { + "x": 6.091139829282408, + "y": 2.286807364004629 + }, + "prevControl": { + "x": 5.3060533457174754, + "y": 2.3267179618763683 + }, + "nextControl": { + "x": 6.766140986689816, + "y": 2.2524930555555556 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.710210937499999, + "y": 2.6393983651620374 + }, + "prevControl": { + "x": 7.147535011574075, + "y": 2.417831597222222 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4914268092105266, + "rotationDegrees": 54.84095812282095 + }, + { + "waypointRelativePos": 0.8793585526315789, + "rotationDegrees": 179.17582565489303 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Neutral Auto 2", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 001e6042..073c4134 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -3,8 +3,8 @@ "robotLength": 0.7874, "holonomicMode": true, "pathFolders": [ - "4 - Hub, Neutral, Hub (HNH)", - "Neutral Auto 1" + "Neutral Auto 1", + "Neutral Auto 2" ], "autoFolders": [], "defaultMaxVel": 3.0, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cb73c705..11c4aff6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -16,7 +16,6 @@ import com.btwrobotics.WhatTime.frc.DriverStation.MatchTimeManager; import com.btwrobotics.WhatTime.frc.YearlyMethods.Rebuilt.RebuiltHubManager; import com.ctre.phoenix6.HootAutoReplay; -import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; diff --git a/src/main/java/frc/robot/joysticks/OperatorJoystick.java b/src/main/java/frc/robot/joysticks/OperatorJoystick.java index 25c147aa..72a94427 100644 --- a/src/main/java/frc/robot/joysticks/OperatorJoystick.java +++ b/src/main/java/frc/robot/joysticks/OperatorJoystick.java @@ -2,9 +2,11 @@ import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.mechanisms.feeder.FeederSubsystem; +import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; @@ -43,23 +45,38 @@ public void configureBindings() { joystick.y(); - // MARK: Shooter accelerate - joystick.rightTrigger(); + // MARK: Intake In - LT + joystick.leftTrigger().onTrue( + // Toggle state of the intake + Commands.runOnce( + () -> { + if (intakeSubsystem.getIntakeState().equals(IntakeStates.INTAKE_IN)) { + intakeSubsystem.setIntake(IntakeStates.OFF); + } + else { + intakeSubsystem.setIntake(IntakeStates.INTAKE_IN); + } + } + ) + ); + + // MARK: Intake Out - LB + joystick.leftBumper().whileTrue( + Commands.runEnd( + () -> { + intakeSubsystem.setIntake(IntakeStates.OFF); + }, + () -> { + // Reset intake to last state. + intakeSubsystem.setIntake(intakeSubsystem.lastIntakeState); + } + ) + ); - // MARK: Intake in - // joystick.leftTrigger().whileTrue( - // Commands.runEnd( - // () -> intakeSubsystem.setIntake(IntakeStates.INTAKE_IN), - // () -> intakeSubsystem.setIntake(IntakeStates.OFF), - // intakeSubsystem - // ) - // ); + joystick.rightTrigger(); - // MARK: Shooter feeder joystick.rightBumper(); - joystick.leftBumper(); - joystick.povUp(); joystick.povDown(); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 7d05f774..818152a4 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -33,6 +33,7 @@ public IntakeSubsystem() { // MARK: Intake Wheel State private IntakeStates intakeState = IntakeStates.OFF; + public IntakeStates lastIntakeState = IntakeStates.OFF; // MARK: Periodic Loop @Override @@ -89,7 +90,15 @@ public void setPosition(double targetPosition) { angleTarget = targetPosition; } + public IntakeStates getIntakeState() { + return intakeState; + } + public void setIntake(IntakeStates intakeState) { + if (!intakeState.equals(this.intakeState)) { + lastIntakeState = this.intakeState; + } + this.intakeState = intakeState; } } From b4e2d591a05554cb83e49d33a90f2277b26a8ca7 Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Thu, 19 Mar 2026 11:32:08 -0500 Subject: [PATCH 089/102] stuff --- src/main/java/frc/robot/subsystems/drive/Drive.java | 2 ++ .../robot/subsystems/mechanisms/feeder/FeederSubsystem.java | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 81a1c6a7..5f687785 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -90,6 +90,8 @@ public void periodic() { Logger.recordOutput("SwerveDrive/TargetHubAngle", getAngleToHub()); + Logger.recordOutput("SwerveDrive/DistanceToHub", getDistanceToHub()); + logMotorInformation(); } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java index ae223324..37e054e0 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/feeder/FeederSubsystem.java @@ -43,11 +43,11 @@ public void periodic() { switch (feederState) { case ALL_FEEDER_IN: - runSpecifiedMotors(true, false, true, false, true, false); + runSpecifiedMotors(true, false, true, false, true, true); break; case ALL_FEEDER_OUT: - runSpecifiedMotors(true, true, true, true, true, true); + runSpecifiedMotors(true, true, true, true, true, false); break; case SHOOTER_FEED_IN: From 9cca8de3199bc9bbe02f7691bb0b1c31957a91ec Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Thu, 19 Mar 2026 12:36:46 -0500 Subject: [PATCH 090/102] Adjusted shooter stuff. --- src/main/java/frc/robot/BuildConstants.java | 12 ++++++------ src/main/java/frc/robot/joysticks/DebugJoystick.java | 2 +- .../mechanisms/shooter/ShooterConstants.java | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index 8bd58fe9..c767d735 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -5,14 +5,14 @@ */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026 Roomba"; + public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 211; - public static final String GIT_SHA = "3c8ccf0aea9ee1a39d453cf0894d68f7beff5854"; - public static final String GIT_DATE = "2026-03-18 11:57:34 CDT"; + public static final int GIT_REVISION = 215; + public static final String GIT_SHA = "29fc91079867ea49140e1c1e22187546c853cef0"; + public static final String GIT_DATE = "2026-03-19 11:32:21 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-19 10:37:48 CDT"; - public static final long BUILD_UNIX_TIME = 1773934668869L; + public static final String BUILD_DATE = "2026-03-19 11:36:25 CDT"; + public static final long BUILD_UNIX_TIME = 1773938185214L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index b68d8130..e9e72205 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -113,7 +113,7 @@ public void configureBindings() { joystick.leftBumper().onTrue( Commands.runOnce( () -> { - shooterSubsystem.shooterPitchMotor.set(50); + shooterSubsystem.shooterPitchMotor.goTo(50); } ) ); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java index 18fd42e7..270d3b08 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java @@ -43,7 +43,7 @@ public class ShooterConstants { public static final double FEEDER_IN_SPEED = 0.2; public static final double FEEDER_OUT_SPEED = -0.05; - public static final double SHOOTER_MIN_ANGLE = 5; + public static final double SHOOTER_MIN_ANGLE = 40; public static final double SHOOTER_MAX_ANGLE = 65; From 10763c1ff39deab9dcf6a562c7fb578946b9a7e4 Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Thu, 19 Mar 2026 12:39:02 -0500 Subject: [PATCH 091/102] Changes to named commands and operator joystick. --- .../commands/namedcommands/RegisterCommands.java | 8 ++++---- .../java/frc/robot/joysticks/OperatorJoystick.java | 13 ++++++++++--- 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java index 911341ef..0afd4442 100644 --- a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java +++ b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java @@ -55,10 +55,10 @@ public void registerCommands(){ // MARK: IntakeAgitate NamedCommands.registerCommand("IntakeAgitate", - Commands.runOnce( - () -> { - intakeSubsystem.setPosition(IntakeConstants.INTAKE_MAX_VALUE/2); - } + Commands.sequence( + Commands.runOnce(() -> intakeSubsystem.setPosition(IntakeConstants.INTAKE_MAX_VALUE / 2)), + Commands.waitSeconds(0.75), + Commands.runOnce(() -> intakeSubsystem.setPosition(IntakeConstants.INTAKE_MIN_VALUE)) ) ); diff --git a/src/main/java/frc/robot/joysticks/OperatorJoystick.java b/src/main/java/frc/robot/joysticks/OperatorJoystick.java index 72a94427..19dff221 100644 --- a/src/main/java/frc/robot/joysticks/OperatorJoystick.java +++ b/src/main/java/frc/robot/joysticks/OperatorJoystick.java @@ -34,15 +34,22 @@ public OperatorJoystick( } public void configureBindings() { - // MARK: Intake Down + // MARK: Intake Down - A joystick.a().onTrue( NamedCommands.getCommand("IntakeDown") ); - joystick.b(); + // MARK: Intake Up - B + joystick.b().onTrue( + NamedCommands.getCommand("IntakeUp") + ); - joystick.x(); + // MARK: Jostle Fuel - X + joystick.x().onTrue( + NamedCommands.getCommand("IntakeAgitate") + ); + // MARK: nothing - Y joystick.y(); // MARK: Intake In - LT From 3e226e948cb350ca063e1b08f3641f6c6d06d54a Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Thu, 19 Mar 2026 13:35:30 -0500 Subject: [PATCH 092/102] Minor auto changes. Moved speed and pitch control from debug joystick to shooter subsystem. Switched back to full point interpolation for the shooter pitch and speed. --- .../pathplanner/autos/Neutral Auto 1.auto | 4 +- .../paths/Neutral Auto 1 - Part 2.path | 79 +++++++++++ .../frc/robot/joysticks/DebugJoystick.java | 40 ++---- .../mechanisms/intake/IntakeSubsystem.java | 13 +- .../mechanisms/shooter/ShooterConstants.java | 14 -- .../mechanisms/shooter/ShooterSubsystem.java | 127 +++++++++++------- 6 files changed, 182 insertions(+), 95 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 2.path diff --git a/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto b/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto index 4b173f77..9579b77e 100644 --- a/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto +++ b/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto @@ -19,13 +19,13 @@ { "type": "path", "data": { - "pathName": "Old Neutral Auto 1 - Part 2" + "pathName": null } }, { "type": "path", "data": { - "pathName": "Neutral Auto 1 - Part 3" + "pathName": null } }, { diff --git a/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 2.path b/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 2.path new file mode 100644 index 00000000..d40f8182 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Neutral Auto 1 - Part 2.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.538508174189815, + "y": 2.361668981481482 + }, + "prevControl": null, + "nextControl": { + "x": 7.875992484880923, + "y": 2.2896624619841495 + }, + "isLocked": false, + "linkedName": "Neutral Auto 1 - Point 2" + }, + { + "anchor": { + "x": 7.160853949652778, + "y": 7.029202256944444 + }, + "prevControl": { + "x": 6.6386748822686785, + "y": 7.591245387179791 + }, + "nextControl": { + "x": 7.80660430720271, + "y": 6.334154188230901 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.538508174189815, + "y": 2.361668981481482 + }, + "prevControl": { + "x": 7.389464795846691, + "y": 3.8473959916429026 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Neutral Auto 1 - Point 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.1729029605263157, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 1.0609580592105259, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Neutral Auto 1", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index e9e72205..8f7df60e 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -4,7 +4,6 @@ import com.pathplanner.lib.auto.NamedCommands; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -24,8 +23,6 @@ public class DebugJoystick { private final IntakeSubsystem intakeSubsystem; private final FeederSubsystem feederSubsystem; - private double shooterSpeed; - private double shooterPitch; public DebugJoystick( CommandXboxController joystick, @@ -40,29 +37,6 @@ public DebugJoystick( this.intakeSubsystem = intakeSubsystem; this.feederSubsystem = feederSubsystem; - this.shooterSpeed = 0.0; - this.shooterPitch = 0.0; - - shooterSubsystem.setDefaultCommand( - Commands.run( - () -> { - - double leftJoystickValue = Math.abs(joystick.getLeftY()) > 0.05 ? -joystick.getLeftY(): 0.0; - double rightJoystickValue = Math.abs(joystick.getRightY()) > 0.05 ? -joystick.getRightY(): 0.0; - - double changeAmountPerTick = 0.0025; - double change = Math.signum(leftJoystickValue) * changeAmountPerTick; - shooterSpeed = MathUtil.clamp(shooterSpeed + change, -0.1, 1.0); - - shooterPitch = MathUtil.clamp(shooterPitch + Math.signum(rightJoystickValue) * changeAmountPerTick, 5.0 ,65.0); - - shooterSubsystem.shooterMotors.drive(shooterSpeed); - - Logger.recordOutput("ShooterSubsystem/ShooterSpeed", shooterSpeed); - Logger.recordOutput("ShooterSubsystem/FeederSpeed", rightJoystickValue); - }, shooterSubsystem - ) - ); // intakeSubsystem.setDefaultCommand( // Commands.run( @@ -77,6 +51,18 @@ public DebugJoystick( } public void configureBindings() { + // Continuously adjust flywheel speed (left Y) and pitch target (right Y). + // No subsystem requirement — just updates the shared values; the subsystem's + // default command picks them up and calls the motors. + Commands.run(() -> { + double changeAmountPerTick = 0.0025; + double leftY = Math.abs(joystick.getLeftY()) > 0.05 ? -joystick.getLeftY() : 0.0; + double rightY = Math.abs(joystick.getRightY()) > 0.05 ? -joystick.getRightY() : 0.0; + shooterSubsystem.setFlywheelSpeed(shooterSubsystem.flywheelSpeed + Math.signum(leftY) * changeAmountPerTick); + shooterSubsystem.setPitchTarget(shooterSubsystem.pitchTarget + Math.signum(rightY) * changeAmountPerTick); + }).schedule(); + + joystick.a().onTrue( NamedCommands.getCommand("IntakeDown") ); @@ -113,7 +99,7 @@ public void configureBindings() { joystick.leftBumper().onTrue( Commands.runOnce( () -> { - shooterSubsystem.shooterPitchMotor.goTo(50); + shooterSubsystem.setPitchTarget(50); } ) ); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 818152a4..463f8960 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -41,10 +41,7 @@ public void periodic() { runAngleControl(); runIntakeWheels(); - Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getMotor().getPosition().refresh().getValueAsDouble()); - Logger.recordOutput("IntakeSubsystem/AngleTarget", Double.isNaN(angleTarget) ? -1.0 : angleTarget); - Logger.recordOutput("IntakeSubsystem/AngleSpeed", angleMotor.getMotor().get()); - Logger.recordOutput("IntakeSubsystem/WheelState", intakeState.toString()); + logValues(); } // MARK: Angle Control @@ -101,4 +98,12 @@ public void setIntake(IntakeStates intakeState) { this.intakeState = intakeState; } + + // MARK: Logging + private void logValues() { + Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getMotor().getPosition().refresh().getValueAsDouble()); + Logger.recordOutput("IntakeSubsystem/AngleTarget", Double.isNaN(angleTarget) ? -1.0 : angleTarget); + Logger.recordOutput("IntakeSubsystem/AngleSpeed", angleMotor.getMotor().get()); + Logger.recordOutput("IntakeSubsystem/WheelState", intakeState.toString()); + } } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java index 270d3b08..e5abf7ed 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterConstants.java @@ -4,20 +4,6 @@ public class ShooterConstants { public static final List shooterDataPoints = List.of( - // TODO: Collect successful data points and add them here - // new ShooterDataPoint(0.0, 0, 0), - // new ShooterDataPoint(0.5, 0, 0), - // new ShooterDataPoint(1.0, 0, 0), - // new ShooterDataPoint(1.5, 0, 0), - // new ShooterDataPoint(2.0, 0, 0), - // new ShooterDataPoint(2.5, 0, 0), - // new ShooterDataPoint(3.0, 0, 0), - // new ShooterDataPoint(3.5, 0, 0), - // new ShooterDataPoint(4.0, 0, 0), - // new ShooterDataPoint(4.5, 0, 0), - // new ShooterDataPoint(5.0, 0, 0), - // new ShooterDataPoint(5.5, 0, 0), - // new ShooterDataPoint(6.0, 0, 0) new ShooterDataPoint(1.4986, 69.6, 0.65), new ShooterDataPoint(2.775, 45.5, 0.74) ); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 44e6be4d..034df3c1 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.mechanisms.shooter; import java.util.Arrays; +import java.util.Map; import java.util.TreeMap; import org.littletonrobotics.junction.Logger; @@ -11,6 +12,7 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -42,8 +44,7 @@ public class ShooterSubsystem extends SubsystemBase { // public ShooterConstants shooterConstants = new ShooterConstants(); // MARK: Motors - /** Motor controlling the shooter pitch (angle). */ - // this is reversed in real life so its inverted in the code + /** Motor controlling the shooter angle */ public final Motor shooterPitchMotor = new Motor(11, "Mechanisms", true) .setFree(false) .setRange(ShooterConstants.SHOOTER_MIN_ANGLE, ShooterConstants.SHOOTER_MAX_ANGLE) @@ -61,6 +62,9 @@ public class ShooterSubsystem extends SubsystemBase { Arrays.asList(leftShooterMotor, rightShooterMotor) ).setMotorSpeed(0.4); + /** IMU sensor for shooter orientation feedback. */ + public final Pigeon2 shooterPigeon = new Pigeon2(34, "Mechanisms"); + // MARK: Constructor /** * Constructs the ShooterSubsystem. @@ -76,86 +80,104 @@ public ShooterSubsystem(Drive drivetrain) { for (ShooterDataPoint point : ShooterConstants.shooterDataPoints) { dataPoints.put(point.distance, point); } - } - /** IMU sensor for shooter orientation feedback. */ - public final Pigeon2 shooterPigeon = new Pigeon2(34, "Mechanisms"); + setDefaultCommand( + Commands.run(() -> { + if (pitchTarget != lastSentPitchTarget) { + shooterPitchMotor.goTo(pitchTarget); + lastSentPitchTarget = pitchTarget; + } + if (flywheelSpeed != lastSentFlywheelSpeed) { + shooterMotors.drive(flywheelSpeed); + lastSentFlywheelSpeed = flywheelSpeed; + } + }, this) + ); + } - public final CANcoder shooterThroughBore = new CANcoder(36); + public double shooterAngleTarget = 65; - public double shooterAngleTargetTesting = 65; + /** Shared pitch target used by manual joystick control and button bindings. */ + public double pitchTarget = ShooterConstants.SHOOTER_MIN_ANGLE; + private double lastSentPitchTarget = Double.NaN; - @Override - public void periodic() { - Logger.recordOutput("ShooterSubsystem/TestingAngleTarget", shooterAngleTargetTesting); - Logger.recordOutput("ShooterSubsystem/MotorConnected", shooterPitchMotor.getMotor().isConnected()); - Logger.recordOutput("ShooterSubsystem/ThroughBoreAngle", getThroughBorePosition()); - Logger.recordOutput("ShooterSubsystem/PigeonAngle", getPigeonPosition()); - Logger.recordOutput("ShooterSubsystem/PitchMotorOutput", shooterPitchMotor.getMotor().get()); + // MARK: Set Pitch Target + public void setPitchTarget(double pitch) { + pitchTarget = MathUtil.clamp(pitch, ShooterConstants.SHOOTER_MIN_ANGLE, ShooterConstants.SHOOTER_MAX_ANGLE); } - // --- Commands --- \\ + /** Shared flywheel speed target used by manual joystick control and button bindings. */ + public double flywheelSpeed = 0.0; + private double lastSentFlywheelSpeed = Double.NaN; - // MARK: Get Through Bore - public double getThroughBorePosition() { - double offset = 0.4; - return shooterThroughBore.getAbsolutePosition().refresh().getValueAsDouble() + offset; + // MARK: Set Flywheel + public void setFlywheelSpeed(double speed) { + flywheelSpeed = MathUtil.clamp(speed, -0.1, 1.0); } + // MARK: Periodic Loop + @Override + public void periodic() { + logValues(); + } + + // MARK: Get Pigeon public double getPigeonPosition() { return shooterPigeon.getRoll().refresh().getValueAsDouble() * -1; } - // For testing shooter angle manually + // MARK: Increment Shooter + /** For testing shooter angle manually */ public void incrementShooterAngle(double incrementValue) { - shooterAngleTargetTesting += incrementValue; + shooterAngleTarget += incrementValue; - shooterPitchMotor.goTo(shooterAngleTargetTesting); + shooterPitchMotor.goTo(shooterAngleTarget); } + // MARK: UpperLowerPoint public UpperLowerPoint shooterUpperLower() { // MARK: NEEDS REFACTORING // if (dataPoints.isEmpty()) { - double currentDistance = drivetrain.getDistanceToHub(); - double aimHeight = (6 - 20 / 12) / 3.281; + // double currentDistance = drivetrain.getDistanceToHub(); + // double aimHeight = (6 - 20 / 12) / 3.281; - double[] trajectory = (new MathSubsystem()).calculateTrajectoryFromExitAngle(currentDistance, aimHeight, 70); + // double[] trajectory = (new MathSubsystem()).calculateTrajectoryFromExitAngle(currentDistance, aimHeight, 70); - return new UpperLowerPoint( - new ShooterDataPoint(currentDistance, trajectory[1], trajectory[0]), - new ShooterDataPoint(currentDistance, trajectory[1], trajectory[0]) - ); + // return new UpperLowerPoint( + // new ShooterDataPoint(currentDistance, trajectory[1], trajectory[0]), + // new ShooterDataPoint(currentDistance, trajectory[1], trajectory[0]) + // ); // } - // double currentDistance = drivetrain.getDistanceToHub(); - + + double currentDistance = drivetrain.getDistanceToHub(); - // Map.Entry lowerEntry = dataPoints.floorEntry(currentDistance); - // Map.Entry upperEntry = dataPoints.ceilingEntry(currentDistance); + Map.Entry lowerEntry = dataPoints.floorEntry(currentDistance); + Map.Entry upperEntry = dataPoints.ceilingEntry(currentDistance); - // // Handle out-of-range cases by clamping to the nearest point - // ShooterDataPoint lower = (lowerEntry != null) ? lowerEntry.getValue() : upperEntry.getValue(); - // ShooterDataPoint upper = (upperEntry != null) ? upperEntry.getValue() : lowerEntry.getValue(); + // Handle out-of-range cases by clamping to the nearest point + ShooterDataPoint lower = (lowerEntry != null) ? lowerEntry.getValue() : upperEntry.getValue(); + ShooterDataPoint upper = (upperEntry != null) ? upperEntry.getValue() : lowerEntry.getValue(); - // return new UpperLowerPoint(upper, lower); + return new UpperLowerPoint(upper, lower); } public ShooterDataPoint calculateShooterValues(UpperLowerPoint upperLowerPoint, double currentDistance) { - // double valueRange = Math.abs(upperLowerPoint.getUpperDistance() - upperLowerPoint.getLowerDistance()); - // if (valueRange == 0) { - return new ShooterDataPoint(currentDistance, upperLowerPoint.getUpperAngle(), upperLowerPoint.getUpperSpeed()); - // } + double lowerDist = upperLowerPoint.getLowerDistance(); + double upperDist = upperLowerPoint.getUpperDistance(); + double valueRange = upperDist - lowerDist; - // double scaledValue = currentDistance - Math.min(upperLowerPoint.getUpperDistance(), upperLowerPoint.getLowerDistance()); + if (valueRange == 0) { + return new ShooterDataPoint(currentDistance, upperLowerPoint.getLowerAngle(), upperLowerPoint.getLowerSpeed()); + } - // double interpolationFactor = scaledValue/valueRange; + double interpolationFactor = (currentDistance - lowerDist) / valueRange; - // // Interpolate the angle and speed between the data points - // double estimatedAngle = upperLowerPoint.getLowerAngle() + interpolationFactor * (upperLowerPoint.getUpperAngle() - upperLowerPoint.getLowerAngle()); - // double estimatedSpeed = upperLowerPoint.getLowerSpeed() + interpolationFactor * (upperLowerPoint.getUpperSpeed() - upperLowerPoint.getLowerSpeed()); - - // return new ShooterDataPoint(currentDistance, estimatedAngle, estimatedSpeed); + double estimatedAngle = upperLowerPoint.getLowerAngle() + interpolationFactor * (upperLowerPoint.getUpperAngle() - upperLowerPoint.getLowerAngle()); + double estimatedSpeed = upperLowerPoint.getLowerSpeed() + interpolationFactor * (upperLowerPoint.getUpperSpeed() - upperLowerPoint.getLowerSpeed()); + + return new ShooterDataPoint(currentDistance, estimatedAngle, estimatedSpeed); } public Command accelerateToSpeed(double targetSpeed) { @@ -174,4 +196,13 @@ public double getRequiredRPM(ShooterDataPoint shooterDataPoint){ double vWheel = 2 * shooterDataPoint.speed; return vWheel * 60 / (Math.PI * 4 * 0.0254); // get rpm required for wheel with diameter of 4 inches } + + // MARK: Logging + private void logValues() { + Logger.recordOutput("ShooterSubsystem/MotorConnected", shooterPitchMotor.getMotor().isConnected()); + Logger.recordOutput("ShooterSubsystem/PigeonAngle", getPigeonPosition()); + Logger.recordOutput("ShooterSubsystem/PitchMotorOutput", shooterPitchMotor.getMotor().get()); + Logger.recordOutput("ShooterSubsystem/ShooterSpeed", flywheelSpeed); + Logger.recordOutput("ShooterSubsystem/TargetAngle", shooterAngleTarget); + } } From 8ca7749c403ba6ffc2bb53940a93289ba9eb571b Mon Sep 17 00:00:00 2001 From: Caedmon Myers Date: Thu, 19 Mar 2026 15:48:01 -0500 Subject: [PATCH 093/102] controls --- .../frc/robot/joysticks/OperatorJoystick.java | 25 +++++++++++++++-- .../frc/robot/subsystems/drive/Drive.java | 27 ++++++++++--------- .../mechanisms/shooter/ShooterSubsystem.java | 9 ++++++- 3 files changed, 46 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/OperatorJoystick.java b/src/main/java/frc/robot/joysticks/OperatorJoystick.java index 19dff221..354c3ebd 100644 --- a/src/main/java/frc/robot/joysticks/OperatorJoystick.java +++ b/src/main/java/frc/robot/joysticks/OperatorJoystick.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.mechanisms.feeder.FeederState; import frc.robot.subsystems.mechanisms.feeder.FeederSubsystem; import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; @@ -80,9 +81,29 @@ public void configureBindings() { ) ); - joystick.rightTrigger(); + // MARK: Shoot Feed In - RT + joystick.rightTrigger().whileTrue( + Commands.runEnd( + () -> { + feederSubsystem.setFeederState(FeederState.ALL_FEEDER_IN); + }, + () -> { + feederSubsystem.setFeederState(FeederState.OFF); + } + ) + ); - joystick.rightBumper(); + // MARK: Shoot Feed Out - RB + joystick.rightBumper().whileTrue( + Commands.runEnd( + () -> { + feederSubsystem.setFeederState(FeederState.ALL_FEEDER_OUT); + }, + () -> { + feederSubsystem.setFeederState(FeederState.OFF); + } + ) + ); joystick.povUp(); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 5f687785..950daccb 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -80,18 +80,7 @@ public Drive(CommandSwerveDrivetrain drivetrain) { public void periodic() { drivetrain.periodic(); - Logger.recordOutput("SwerveDrive/Pose", getPose2d()); - - // Log module states for AdvantageScope swerve visualization - Logger.recordOutput("SwerveDrive/ModuleStates", drivetrain.getState().ModuleStates); - Logger.recordOutput("SwerveDrive/ModuleTargets", drivetrain.getState().ModuleTargets); - Logger.recordOutput("SwerveDrive/ChassisSpeeds", drivetrain.getState().Speeds); - Logger.recordOutput("SwerveDrive/Rotation", getPose2d().getRotation()); - - Logger.recordOutput("SwerveDrive/TargetHubAngle", getAngleToHub()); - - Logger.recordOutput("SwerveDrive/DistanceToHub", getDistanceToHub()); - + logValues(); logMotorInformation(); } @@ -271,6 +260,20 @@ public static Pose2d flipAlliance(Pose2d pose) { return pose; } + // MARK: Logging + private void logValues() { + Logger.recordOutput("SwerveDrive/Pose", getPose2d()); + + // Log module states for AdvantageScope swerve visualization + Logger.recordOutput("SwerveDrive/ModuleStates", drivetrain.getState().ModuleStates); + Logger.recordOutput("SwerveDrive/ModuleTargets", drivetrain.getState().ModuleTargets); + Logger.recordOutput("SwerveDrive/ChassisSpeeds", drivetrain.getState().Speeds); + Logger.recordOutput("SwerveDrive/Rotation", getPose2d().getRotation()); + + Logger.recordOutput("SwerveDrive/TargetHubAngle", getAngleToHub()); + Logger.recordOutput("SwerveDrive/DistanceToHub", getDistanceToHub()); + } + // MARK: Motor Logging public void logMotorInformation() { for (int i = 0; i <= 3; i++) { diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 034df3c1..b59032fc 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -118,6 +118,13 @@ public void setFlywheelSpeed(double speed) { // MARK: Periodic Loop @Override public void periodic() { + if (drivetrain.isLockedToHub()) { + double currentDistance = drivetrain.getDistanceToHub(); + ShooterDataPoint values = calculateShooterValues(shooterUpperLower(), currentDistance); + setPitchTarget(values.angle); + setFlywheelSpeed(values.speed); + } + logValues(); } @@ -186,7 +193,7 @@ public Command accelerateToSpeed(double targetSpeed) { () -> timer.restart(), () -> { double rampedSpeed = Math.min(timer.get() / 5.0, 1.0) * targetSpeed; - shooterMotors.drive(rampedSpeed); + setFlywheelSpeed(rampedSpeed); }, this ).until(() -> timer.hasElapsed(5.0)); From 87be7347bbe1f11883076863ee5ba7e42d0f9b88 Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Thu, 19 Mar 2026 16:58:41 -0500 Subject: [PATCH 094/102] changes --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- .../subsystems/mechanisms/intake/IntakeConstants.java | 4 ++-- .../subsystems/mechanisms/intake/IntakeSubsystem.java | 1 + .../mechanisms/shooter/ShooterSubsystem.java | 10 +++++----- 4 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index c767d735..de17060b 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 215; - public static final String GIT_SHA = "29fc91079867ea49140e1c1e22187546c853cef0"; - public static final String GIT_DATE = "2026-03-19 11:32:21 CDT"; + public static final int GIT_REVISION = 220; + public static final String GIT_SHA = "8ca7749c403ba6ffc2bb53940a93289ba9eb571b"; + public static final String GIT_DATE = "2026-03-19 15:48:01 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-19 11:36:25 CDT"; - public static final long BUILD_UNIX_TIME = 1773938185214L; + public static final String BUILD_DATE = "2026-03-19 16:53:04 CDT"; + public static final long BUILD_UNIX_TIME = 1773957184825L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java index 8b50d183..cd258245 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java @@ -3,9 +3,9 @@ public class IntakeConstants { public static final double INTAKE_MIN_VALUE = 0.0; public static final double INTAKE_MAX_VALUE = 2.34; - public static final double INTAKE_THRESHOLD = 0.4; + public static final double INTAKE_THRESHOLD = 0.2; public static final double INTAKE_WHEELS_SPEED = 0.5; - public static final double INTAKE_DOWN_SPEED = 0.2; + public static final double INTAKE_DOWN_SPEED = 0.3; public static final double INTAKE_UP_SPEED = 0.2; } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 463f8960..bd80e6a8 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -105,5 +105,6 @@ private void logValues() { Logger.recordOutput("IntakeSubsystem/AngleTarget", Double.isNaN(angleTarget) ? -1.0 : angleTarget); Logger.recordOutput("IntakeSubsystem/AngleSpeed", angleMotor.getMotor().get()); Logger.recordOutput("IntakeSubsystem/WheelState", intakeState.toString()); + Logger.recordOutput("IntakeSubsystem/Current/AngleMotor", angleMotor.getMotor().getStatorCurrent().getValueAsDouble()); } } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index b59032fc..357bf639 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -45,12 +45,12 @@ public class ShooterSubsystem extends SubsystemBase { // MARK: Motors /** Motor controlling the shooter angle */ - public final Motor shooterPitchMotor = new Motor(11, "Mechanisms", true) + public final Motor shooterPitchMotor = new Motor(11, "Mechanisms", false) .setFree(false) .setRange(ShooterConstants.SHOOTER_MIN_ANGLE, ShooterConstants.SHOOTER_MAX_ANGLE) - .setMotorSpeed(0.5) - .setMinSpeed(0.1) - .setHoldSpeed(0.1) + .setMotorSpeed(0.1) + .setMinSpeed(0.0) + .setHoldSpeed(0.0) .setPG(0.02) .setThreshold(ShooterConstants.POSITION_THRESHOLD) .setPositionSupplier(() -> getPigeonPosition()); @@ -98,7 +98,7 @@ public ShooterSubsystem(Drive drivetrain) { public double shooterAngleTarget = 65; /** Shared pitch target used by manual joystick control and button bindings. */ - public double pitchTarget = ShooterConstants.SHOOTER_MIN_ANGLE; + public double pitchTarget = ShooterConstants.SHOOTER_MAX_ANGLE; private double lastSentPitchTarget = Double.NaN; // MARK: Set Pitch Target From eefc42bd15afa008a156d263e43a6ee82d67f1b9 Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Thu, 19 Mar 2026 22:02:40 +0000 Subject: [PATCH 095/102] Remove caedmon's reimplementation of the whattime motor because nobody knows why she made that --- .../mechanisms/intake/IntakeSubsystem.java | 40 +++++++++---------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index bd80e6a8..28b47184 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -38,31 +38,31 @@ public IntakeSubsystem() { // MARK: Periodic Loop @Override public void periodic() { - runAngleControl(); + // runAngleControl(); runIntakeWheels(); logValues(); } // MARK: Angle Control - private void runAngleControl() { - if (Double.isNaN(angleTarget)) { - angleMotor.set(0.0); - return; - } - - double currentPos = angleMotor.getMotor().getPosition().refresh().getValueAsDouble(); - double error = angleTarget - currentPos; - - if (Math.abs(error) <= IntakeConstants.INTAKE_THRESHOLD) { - angleMotor.set(0.0); - } else if (error < 0) { - angleMotor.set(Math.copySign(IntakeConstants.INTAKE_DOWN_SPEED, error)); - } - else { - angleMotor.set(Math.copySign(IntakeConstants.INTAKE_UP_SPEED, error)); - } - } + // private void runAngleControl() { + // if (Double.isNaN(angleTarget)) { + // angleMotor.set(0.0); + // return; + // } + + // double currentPos = angleMotor.getMotor().getPosition().refresh().getValueAsDouble(); + // double error = angleTarget - currentPos; + + // if (Math.abs(error) <= IntakeConstants.INTAKE_THRESHOLD) { + // angleMotor.set(0.0); + // } else if (error < 0) { + // angleMotor.set(Math.copySign(IntakeConstants.INTAKE_DOWN_SPEED, error)); + // } + // else { + // angleMotor.set(Math.copySign(IntakeConstants.INTAKE_UP_SPEED, error)); + // } + // } // MARK: Intake Wheels private void runIntakeWheels() { @@ -84,7 +84,7 @@ private void runIntakeWheels() { public void setPosition(double targetPosition) { Logger.recordOutput("IntakeSubsystem/SetPosition", targetPosition); - angleTarget = targetPosition; + angleMotor.goTo(targetPosition); } public IntakeStates getIntakeState() { From ddac149b330e52d308d7ec98b00f3ad5c4ce9531 Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Thu, 19 Mar 2026 22:08:31 +0000 Subject: [PATCH 096/102] Add min and max to intake motor --- .../robot/subsystems/mechanisms/intake/IntakeSubsystem.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 28b47184..a2144b13 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -11,7 +11,9 @@ public class IntakeSubsystem extends SubsystemBase { // MARK: Intake Angle - public Motor angleMotor = new Motor(9, "Mechanisms"); + public Motor angleMotor = new Motor(9, "Mechanisms") + .setMinValue(0.0) + .setMaxValue(2.34); // public TalonFX angleMotor = new TalonFX(9, "Mechanisms"); // MARK: Intake Wheels From 2f7959d4e3382bb8ce1dad7475d35bd5d45ef9a3 Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Thu, 19 Mar 2026 22:13:34 +0000 Subject: [PATCH 097/102] Update intake commands from runOnce to run --- .../frc/robot/commands/namedcommands/RegisterCommands.java | 4 ++-- .../robot/subsystems/mechanisms/intake/IntakeSubsystem.java | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java index 0afd4442..35291292 100644 --- a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java +++ b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java @@ -37,7 +37,7 @@ public void registerCommands(){ // MARK: IntakeDown NamedCommands.registerCommand("IntakeDown", - Commands.runOnce( + Commands.run( () -> { intakeSubsystem.setPosition(IntakeConstants.INTAKE_MIN_VALUE); } @@ -46,7 +46,7 @@ public void registerCommands(){ // MARK: IntakeUp NamedCommands.registerCommand("IntakeUp", - Commands.runOnce( + Commands.run( () -> { intakeSubsystem.setPosition(IntakeConstants.INTAKE_MAX_VALUE); } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index a2144b13..a0a6da10 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -13,7 +13,8 @@ public class IntakeSubsystem extends SubsystemBase { // MARK: Intake Angle public Motor angleMotor = new Motor(9, "Mechanisms") .setMinValue(0.0) - .setMaxValue(2.34); + .setMaxValue(2.34) + .setThreshold(0.2); // public TalonFX angleMotor = new TalonFX(9, "Mechanisms"); // MARK: Intake Wheels From d3c4e0549b995dd518dc30701cce2d76c1e25a9c Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Thu, 19 Mar 2026 17:24:01 -0500 Subject: [PATCH 098/102] did things --- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- .../robot/commands/namedcommands/RegisterCommands.java | 4 ++-- .../subsystems/mechanisms/intake/IntakeSubsystem.java | 3 +++ .../mechanisms/shooter/ShooterSubsystem.java | 2 +- 4 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index de17060b..ba6760c1 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 220; - public static final String GIT_SHA = "8ca7749c403ba6ffc2bb53940a93289ba9eb571b"; - public static final String GIT_DATE = "2026-03-19 15:48:01 CDT"; + public static final int GIT_REVISION = 224; + public static final String GIT_SHA = "2f7959d4e3382bb8ce1dad7475d35bd5d45ef9a3"; + public static final String GIT_DATE = "2026-03-19 17:13:34 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-19 16:53:04 CDT"; - public static final long BUILD_UNIX_TIME = 1773957184825L; + public static final String BUILD_DATE = "2026-03-19 17:23:58 CDT"; + public static final long BUILD_UNIX_TIME = 1773959038658L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java index 35291292..34312c1d 100644 --- a/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java +++ b/src/main/java/frc/robot/commands/namedcommands/RegisterCommands.java @@ -40,7 +40,7 @@ public void registerCommands(){ Commands.run( () -> { intakeSubsystem.setPosition(IntakeConstants.INTAKE_MIN_VALUE); - } + }, intakeSubsystem ) ); @@ -49,7 +49,7 @@ public void registerCommands(){ Commands.run( () -> { intakeSubsystem.setPosition(IntakeConstants.INTAKE_MAX_VALUE); - } + }, intakeSubsystem ) ); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index a0a6da10..908a817b 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -12,11 +12,14 @@ public class IntakeSubsystem extends SubsystemBase { // MARK: Intake Angle public Motor angleMotor = new Motor(9, "Mechanisms") + .setFree(false) + .setMotorSpeed(0.4) .setMinValue(0.0) .setMaxValue(2.34) .setThreshold(0.2); // public TalonFX angleMotor = new TalonFX(9, "Mechanisms"); + // MARK: Intake Wheels public Motor intakeWheelsMotor = new Motor(10, "Mechanisms"); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 357bf639..937b2fcd 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -45,7 +45,7 @@ public class ShooterSubsystem extends SubsystemBase { // MARK: Motors /** Motor controlling the shooter angle */ - public final Motor shooterPitchMotor = new Motor(11, "Mechanisms", false) + public final Motor shooterPitchMotor = new Motor(11, "Mechanisms") .setFree(false) .setRange(ShooterConstants.SHOOTER_MIN_ANGLE, ShooterConstants.SHOOTER_MAX_ANGLE) .setMotorSpeed(0.1) From 209b1eeae03c9d2218320ca15514a69d21dfc494 Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Thu, 19 Mar 2026 22:25:24 +0000 Subject: [PATCH 099/102] Enable intake angle motor --- .../robot/subsystems/mechanisms/intake/IntakeSubsystem.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 908a817b..8cab4a83 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -27,10 +27,9 @@ public class IntakeSubsystem extends SubsystemBase { private double angleTarget = Double.NaN; public IntakeSubsystem() { - // angleMotor.toggleEnabled(true); + angleMotor.toggleEnabled(true); intakeWheelsMotor.toggleEnabled(true); - // angleMotor.setDefaultCommand(Commands.run(() -> {}, angleMotor)); intakeWheelsMotor.setDefaultCommand(Commands.run(() -> {}, intakeWheelsMotor)); angleMotor.getMotor().getConfigurator().apply(RobotContainer.mechanismsMotorConfiguration); From 2a612d1df764431241bba4f0ffbdc619c750a4c9 Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Thu, 19 Mar 2026 22:49:15 +0000 Subject: [PATCH 100/102] Fix shooter motors --- .../frc/robot/joysticks/DebugJoystick.java | 1 + .../frc/robot/joysticks/OperatorJoystick.java | 20 +++++++++---------- .../mechanisms/intake/IntakeSubsystem.java | 5 +---- .../mechanisms/shooter/ShooterSubsystem.java | 8 +++++--- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 8f7df60e..e5337f6f 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -58,6 +58,7 @@ public void configureBindings() { double changeAmountPerTick = 0.0025; double leftY = Math.abs(joystick.getLeftY()) > 0.05 ? -joystick.getLeftY() : 0.0; double rightY = Math.abs(joystick.getRightY()) > 0.05 ? -joystick.getRightY() : 0.0; + shooterSubsystem.setFlywheelSpeed(shooterSubsystem.flywheelSpeed + Math.signum(leftY) * changeAmountPerTick); shooterSubsystem.setPitchTarget(shooterSubsystem.pitchTarget + Math.signum(rightY) * changeAmountPerTick); }).schedule(); diff --git a/src/main/java/frc/robot/joysticks/OperatorJoystick.java b/src/main/java/frc/robot/joysticks/OperatorJoystick.java index 354c3ebd..f2078d45 100644 --- a/src/main/java/frc/robot/joysticks/OperatorJoystick.java +++ b/src/main/java/frc/robot/joysticks/OperatorJoystick.java @@ -54,20 +54,20 @@ public void configureBindings() { joystick.y(); // MARK: Intake In - LT - joystick.leftTrigger().onTrue( - // Toggle state of the intake - Commands.runOnce( + joystick.leftTrigger() + .whileTrue( + Commands.runEnd( () -> { - if (intakeSubsystem.getIntakeState().equals(IntakeStates.INTAKE_IN)) { - intakeSubsystem.setIntake(IntakeStates.OFF); - } - else { - intakeSubsystem.setIntake(IntakeStates.INTAKE_IN); - } - } + intakeSubsystem.setIntake(IntakeStates.INTAKE_IN); + }, + () -> { + intakeSubsystem.setIntake(IntakeStates.OFF); + }, + intakeSubsystem ) ); + // MARK: Intake Out - LB joystick.leftBumper().whileTrue( Commands.runEnd( diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 8cab4a83..85bc6402 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -23,9 +23,6 @@ public class IntakeSubsystem extends SubsystemBase { // MARK: Intake Wheels public Motor intakeWheelsMotor = new Motor(10, "Mechanisms"); - // MARK: Angle Control State - private double angleTarget = Double.NaN; - public IntakeSubsystem() { angleMotor.toggleEnabled(true); intakeWheelsMotor.toggleEnabled(true); @@ -107,7 +104,7 @@ public void setIntake(IntakeStates intakeState) { // MARK: Logging private void logValues() { Logger.recordOutput("IntakeSubsystem/Angle", angleMotor.getMotor().getPosition().refresh().getValueAsDouble()); - Logger.recordOutput("IntakeSubsystem/AngleTarget", Double.isNaN(angleTarget) ? -1.0 : angleTarget); + // Logger.recordOutput("IntakeSubsystem/AngleTarget", Double.isNaN(angleTarget) ? -1.0 : angleTarget); Logger.recordOutput("IntakeSubsystem/AngleSpeed", angleMotor.getMotor().get()); Logger.recordOutput("IntakeSubsystem/WheelState", intakeState.toString()); Logger.recordOutput("IntakeSubsystem/Current/AngleMotor", angleMotor.getMotor().getStatorCurrent().getValueAsDouble()); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 937b2fcd..3873903f 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -60,7 +60,8 @@ public class ShooterSubsystem extends SubsystemBase { public final MotorGroup shooterMotors = new MotorGroup( Arrays.asList(leftShooterMotor, rightShooterMotor) - ).setMotorSpeed(0.4); + ).setMotorSpeed(0.4) + .setMinSpeed(-0.1); /** IMU sensor for shooter orientation feedback. */ public final Pigeon2 shooterPigeon = new Pigeon2(34, "Mechanisms"); @@ -103,7 +104,8 @@ public ShooterSubsystem(Drive drivetrain) { // MARK: Set Pitch Target public void setPitchTarget(double pitch) { - pitchTarget = MathUtil.clamp(pitch, ShooterConstants.SHOOTER_MIN_ANGLE, ShooterConstants.SHOOTER_MAX_ANGLE); + // pitchTarget = MathUtil.clamp(pitch, ShooterConstants.SHOOTER_MIN_ANGLE, ShooterConstants.SHOOTER_MAX_ANGLE); + shooterPitchMotor.goTo(pitchTarget); } /** Shared flywheel speed target used by manual joystick control and button bindings. */ @@ -112,7 +114,7 @@ public void setPitchTarget(double pitch) { // MARK: Set Flywheel public void setFlywheelSpeed(double speed) { - flywheelSpeed = MathUtil.clamp(speed, -0.1, 1.0); + shooterMotors.drive(speed); } // MARK: Periodic Loop From f4ba87c2d6f7a5b07ba77b458e5dbf8cc4f11f48 Mon Sep 17 00:00:00 2001 From: Tullysaurus Date: Thu, 19 Mar 2026 23:10:24 +0000 Subject: [PATCH 101/102] Fixx debug joysticks --- .../java/frc/robot/joysticks/DebugJoystick.java | 9 ++++++--- .../mechanisms/shooter/ShooterSubsystem.java | 16 ++++++++-------- 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index e5337f6f..13135c7d 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -58,9 +58,12 @@ public void configureBindings() { double changeAmountPerTick = 0.0025; double leftY = Math.abs(joystick.getLeftY()) > 0.05 ? -joystick.getLeftY() : 0.0; double rightY = Math.abs(joystick.getRightY()) > 0.05 ? -joystick.getRightY() : 0.0; - - shooterSubsystem.setFlywheelSpeed(shooterSubsystem.flywheelSpeed + Math.signum(leftY) * changeAmountPerTick); - shooterSubsystem.setPitchTarget(shooterSubsystem.pitchTarget + Math.signum(rightY) * changeAmountPerTick); + + + shooterSubsystem.shooterMotors.drive(shooterSubsystem.leftShooterMotor.getCurrentValue() + Math.signum(leftY) * changeAmountPerTick); + shooterSubsystem.shooterPitchMotor.goTo(shooterSubsystem.shooterPitchMotor.getCurrentValue() + + Math.signum(rightY) * changeAmountPerTick); + // shooterSubsystem.setFlywheelSpeed(shooterSubsystem.flywheelSpeed + Math.signum(leftY) * changeAmountPerTick); + // shooterSubsystem.setPitchTarget(shooterSubsystem.pitchTarget + Math.signum(rightY) * changeAmountPerTick); }).schedule(); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index 3873903f..a89f0163 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -105,7 +105,7 @@ public ShooterSubsystem(Drive drivetrain) { // MARK: Set Pitch Target public void setPitchTarget(double pitch) { // pitchTarget = MathUtil.clamp(pitch, ShooterConstants.SHOOTER_MIN_ANGLE, ShooterConstants.SHOOTER_MAX_ANGLE); - shooterPitchMotor.goTo(pitchTarget); + shooterPitchMotor.goTo(pitchTarget / 180); } /** Shared flywheel speed target used by manual joystick control and button bindings. */ @@ -148,16 +148,16 @@ public UpperLowerPoint shooterUpperLower() { // MARK: NEEDS REFACTORING // if (dataPoints.isEmpty()) { - // double currentDistance = drivetrain.getDistanceToHub(); - // double aimHeight = (6 - 20 / 12) / 3.281; + // double currentDistance = drivetrain.getDistanceToHub(); + // double aimHeight = (6 - 20 / 12) / 3.281; - // double[] trajectory = (new MathSubsystem()).calculateTrajectoryFromExitAngle(currentDistance, aimHeight, 70); + // double[] trajectory = (new MathSubsystem()).calculateTrajectoryFromExitAngle(currentDistance, aimHeight, 70); - // return new UpperLowerPoint( - // new ShooterDataPoint(currentDistance, trajectory[1], trajectory[0]), - // new ShooterDataPoint(currentDistance, trajectory[1], trajectory[0]) - // ); + // return new UpperLowerPoint( + // new ShooterDataPoint(currentDistance, trajectory[1], trajectory[0]), + // new ShooterDataPoint(currentDistance, trajectory[1], trajectory[0]) + // ); // } double currentDistance = drivetrain.getDistanceToHub(); From 20c1fca0bfe51590ccfc1a1ef1b6dd731170e0ca Mon Sep 17 00:00:00 2001 From: 1209robotics <1209robohornets@gmail.com> Date: Fri, 20 Mar 2026 08:21:43 -0500 Subject: [PATCH 102/102] Changes to controls and shooter stuff --- src/main/deploy/pathplanner/autos/Neutral Auto 1.auto | 2 +- src/main/java/frc/robot/BuildConstants.java | 10 +++++----- src/main/java/frc/robot/joysticks/DebugJoystick.java | 6 ++++-- src/main/java/frc/robot/joysticks/DriverJoystick.java | 5 +++-- .../java/frc/robot/joysticks/OperatorJoystick.java | 9 ++++++++- .../subsystems/mechanisms/intake/IntakeConstants.java | 2 +- .../subsystems/mechanisms/intake/IntakeSubsystem.java | 2 +- .../mechanisms/shooter/ShooterSubsystem.java | 2 +- 8 files changed, 24 insertions(+), 14 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto b/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto index 9579b77e..36c5b6a3 100644 --- a/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto +++ b/src/main/deploy/pathplanner/autos/Neutral Auto 1.auto @@ -19,7 +19,7 @@ { "type": "path", "data": { - "pathName": null + "pathName": "Neutral Auto 1 - Part 2" } }, { diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index ba6760c1..90cd7f56 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "roomba"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 224; - public static final String GIT_SHA = "2f7959d4e3382bb8ce1dad7475d35bd5d45ef9a3"; - public static final String GIT_DATE = "2026-03-19 17:13:34 CDT"; + public static final int GIT_REVISION = 228; + public static final String GIT_SHA = "f4ba87c2d6f7a5b07ba77b458e5dbf8cc4f11f48"; + public static final String GIT_DATE = "2026-03-19 18:10:24 CDT"; public static final String GIT_BRANCH = "Whattime-refactor"; - public static final String BUILD_DATE = "2026-03-19 17:23:58 CDT"; - public static final long BUILD_UNIX_TIME = 1773959038658L; + public static final String BUILD_DATE = "2026-03-20 08:18:59 CDT"; + public static final long BUILD_UNIX_TIME = 1774012739222L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/joysticks/DebugJoystick.java b/src/main/java/frc/robot/joysticks/DebugJoystick.java index 13135c7d..e39ad1e5 100644 --- a/src/main/java/frc/robot/joysticks/DebugJoystick.java +++ b/src/main/java/frc/robot/joysticks/DebugJoystick.java @@ -59,9 +59,11 @@ public void configureBindings() { double leftY = Math.abs(joystick.getLeftY()) > 0.05 ? -joystick.getLeftY() : 0.0; double rightY = Math.abs(joystick.getRightY()) > 0.05 ? -joystick.getRightY() : 0.0; + // shooterSubsystem.shooterMotors.drive(leftY); + - shooterSubsystem.shooterMotors.drive(shooterSubsystem.leftShooterMotor.getCurrentValue() + Math.signum(leftY) * changeAmountPerTick); - shooterSubsystem.shooterPitchMotor.goTo(shooterSubsystem.shooterPitchMotor.getCurrentValue() + + Math.signum(rightY) * changeAmountPerTick); + // shooterSubsystem.shooterMotors.drive(shooterSubsystem.leftShooterMotor.getCurrentValue() + Math.signum(leftY) * changeAmountPerTick); + // shooterSubsystem.shooterPitchMotor.goTo(shooterSubsystem.shooterPitchMotor.getCurrentValue() + + Math.signum(rightY) * changeAmountPerTick); // shooterSubsystem.setFlywheelSpeed(shooterSubsystem.flywheelSpeed + Math.signum(leftY) * changeAmountPerTick); // shooterSubsystem.setPitchTarget(shooterSubsystem.pitchTarget + Math.signum(rightY) * changeAmountPerTick); }).schedule(); diff --git a/src/main/java/frc/robot/joysticks/DriverJoystick.java b/src/main/java/frc/robot/joysticks/DriverJoystick.java index 26d736c4..a7797b72 100644 --- a/src/main/java/frc/robot/joysticks/DriverJoystick.java +++ b/src/main/java/frc/robot/joysticks/DriverJoystick.java @@ -8,6 +8,7 @@ import frc.robot.subsystems.mechanisms.feeder.FeederSubsystem; import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; +import frc.robot.subsystems.mechanisms.shooter.ShooterConstants; import frc.robot.subsystems.mechanisms.shooter.ShooterDataPoint; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; @@ -67,7 +68,7 @@ public void configureBindings() { double rpm = shooterSubsystem.getRequiredRPM(shooterDataPoint); - double maxRPM = 1000; // MARK: Populate max rpm + double maxRPM = 200; // MARK: Populate max rpm shooterDataPoint.speed = rpm / maxRPM; shooterDataPoint.angle = shooterDataPoint.angle / 180; // angle (0.5 = 180deg) @@ -98,7 +99,7 @@ public void configureBindings() { shooterSubsystem.shooterMotors.drive(shooterDataPoint.speed); feederSubsystem.shooterFeederMotor.drive(shooterDataPoint.speed); - shooterSubsystem.shooterPitchMotor.goTo(0.0); + shooterSubsystem.shooterPitchMotor.goTo(ShooterConstants.SHOOTER_MAX_ANGLE); Logger.recordOutput("DriverJoystick/ShooterSpeed", shooterDataPoint.speed); Logger.recordOutput("DriverJoystick/ShooterPitch", shooterDataPoint.angle); diff --git a/src/main/java/frc/robot/joysticks/OperatorJoystick.java b/src/main/java/frc/robot/joysticks/OperatorJoystick.java index f2078d45..c6f50c5a 100644 --- a/src/main/java/frc/robot/joysticks/OperatorJoystick.java +++ b/src/main/java/frc/robot/joysticks/OperatorJoystick.java @@ -9,6 +9,7 @@ import frc.robot.subsystems.mechanisms.feeder.FeederSubsystem; import frc.robot.subsystems.mechanisms.intake.IntakeStates; import frc.robot.subsystems.mechanisms.intake.IntakeSubsystem; +import frc.robot.subsystems.mechanisms.shooter.ShooterConstants; import frc.robot.subsystems.mechanisms.shooter.ShooterSubsystem; public class OperatorJoystick { @@ -51,7 +52,13 @@ public void configureBindings() { ); // MARK: nothing - Y - joystick.y(); + joystick.y().whileTrue( + Commands.run( + () -> { + shooterSubsystem.shooterPitchMotor.goTo(ShooterConstants.SHOOTER_MAX_ANGLE); + } + ) + ); // MARK: Intake In - LT joystick.leftTrigger() diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java index cd258245..481dd3ee 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeConstants.java @@ -5,7 +5,7 @@ public class IntakeConstants { public static final double INTAKE_MAX_VALUE = 2.34; public static final double INTAKE_THRESHOLD = 0.2; - public static final double INTAKE_WHEELS_SPEED = 0.5; + public static final double INTAKE_WHEELS_SPEED = 0.75; public static final double INTAKE_DOWN_SPEED = 0.3; public static final double INTAKE_UP_SPEED = 0.2; } diff --git a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java index 85bc6402..1ec3d7f7 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/intake/IntakeSubsystem.java @@ -13,7 +13,7 @@ public class IntakeSubsystem extends SubsystemBase { // MARK: Intake Angle public Motor angleMotor = new Motor(9, "Mechanisms") .setFree(false) - .setMotorSpeed(0.4) + .setMotorSpeed(1.0) .setMinValue(0.0) .setMaxValue(2.34) .setThreshold(0.2); diff --git a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java index a89f0163..9d24ec4e 100644 --- a/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/mechanisms/shooter/ShooterSubsystem.java @@ -61,7 +61,7 @@ public class ShooterSubsystem extends SubsystemBase { public final MotorGroup shooterMotors = new MotorGroup( Arrays.asList(leftShooterMotor, rightShooterMotor) ).setMotorSpeed(0.4) - .setMinSpeed(-0.1); + .setPG(0.01); /** IMU sensor for shooter orientation feedback. */ public final Pigeon2 shooterPigeon = new Pigeon2(34, "Mechanisms");