From b34c8d4221e20387adb5037e1f79ce9ca01f5308 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Tue, 10 Mar 2026 15:21:48 -0500 Subject: [PATCH 01/96] good luck --- .../java/frc/robot/Auto/CenterLemonAuto.java | 2 -- .../java/frc/robot/Auto/DriveTestAuto.java | 2 -- .../frc/robot/Auto/LeftNeutralZoneAuto.java | 22 +++++++++++++++++++ src/main/java/frc/robot/Constants.java | 2 +- 4 files changed, 23 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java diff --git a/src/main/java/frc/robot/Auto/CenterLemonAuto.java b/src/main/java/frc/robot/Auto/CenterLemonAuto.java index f9b2712..92d19ff 100644 --- a/src/main/java/frc/robot/Auto/CenterLemonAuto.java +++ b/src/main/java/frc/robot/Auto/CenterLemonAuto.java @@ -4,12 +4,10 @@ package frc.robot.Auto; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants.ShooterConstants; -import frc.robot.Constants.SwerveConstants; import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.ShooterSubsystem; import frc.robot.Subsystems.SwerveSubsystem; diff --git a/src/main/java/frc/robot/Auto/DriveTestAuto.java b/src/main/java/frc/robot/Auto/DriveTestAuto.java index f563d08..6975029 100644 --- a/src/main/java/frc/robot/Auto/DriveTestAuto.java +++ b/src/main/java/frc/robot/Auto/DriveTestAuto.java @@ -4,8 +4,6 @@ package frc.robot.Auto; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Subsystems.SwerveSubsystem; diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java new file mode 100644 index 0000000..bed5a56 --- /dev/null +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java @@ -0,0 +1,22 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Subsystems.SwerveSubsystem; + + +public class LeftNeutralZoneAuto extends SequentialCommandGroup { + public LeftNeutralZoneAuto (SwerveSubsystem drive) { + addCommands( + new InstantCommand(() -> drive.drive(0.5,0,0, false), drive), + Commands.waitSeconds(2), + new InstantCommand(() -> drive.drive(0,0,0, false), drive) + ); + } +} + diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 416a1ed..8cac2bf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -335,4 +335,4 @@ public static final class CANdleConstants { public static final int CANDLE_ID = 18; //Placeholder ID } -} +} \ No newline at end of file From 0b3a0a6ec6dbb5313443c52f6c0b94beead28a68 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Tue, 10 Mar 2026 15:59:35 -0600 Subject: [PATCH 02/96] Fixed hood angle --- src/main/java/frc/robot/Constants.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8cac2bf..b321e90 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -97,6 +97,7 @@ public static final class SwerveConstants{ /* Swerve Profiling Values */ public static final double maxSpeed = 5; // meters per second + public static final double PathPlannerMaxSpeed = DCMotor.getNeoVortex(1).withReduction(driveGearRatio).freeSpeedRadPerSec*(wheelDiameter/2); public static final double maxAngularVelocity = maxSpeed/driveBaseRadius; //radians per second how fast the robot spin /* Neutral Modes */ @@ -142,7 +143,7 @@ public static final class AutoConstants { private static boolean dashboardInitialized = false; public static final ModuleConfig MODULE_CONFIG = new ModuleConfig(SwerveConstants.wheelDiameter/2, - SwerveConstants.maxSpeed, + SwerveConstants.PathPlannerMaxSpeed, 1.2, DCMotor.getNeoVortex(1).withReduction(SwerveConstants.driveGearRatio), SwerveConstants.driveContinuousCurrentLimit, @@ -156,6 +157,7 @@ public static final class AutoConstants { public enum AutoMode{ None, + DriveTestAuto, LeftLemonAuto, RightLemonAuto, ShootEightAuto, @@ -175,6 +177,7 @@ public static void initDashboard() { autoModeChooser.setDefaultOption("LeftLemonAuto", AutoMode.LeftLemonAuto); autoModeChooser.addOption("None", AutoMode.None); + autoModeChooser.addOption("DriveTestAuto", AutoMode.DriveTestAuto); autoModeChooser.addOption("ShootEightAuto", AutoMode.ShootEightAuto); autoModeChooser.addOption("RightLemonAuto", AutoMode.RightLemonAuto); autoModeChooser.addOption("LeftLemonAuto", AutoMode.LeftLemonAuto); @@ -291,7 +294,7 @@ public static final class ShooterConstants { // Max travel is 3 rotations = 1080 degrees. public static final double HOOD_MIN_ROTATIONS = 0.0; public static final double HOOD_MED_ROTATIONS = 20.0; - public static final double HOOD_MAX_ROTATIONS = 36.0; + public static final double HOOD_MAX_ROTATIONS = 23.0; // Preset positions. public static final double HOOD_ANGLE_LOW = HOOD_MIN_ROTATIONS; From 70330fc8bd8ce832508642fc2e5a53664618b098 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Tue, 10 Mar 2026 19:46:40 -0500 Subject: [PATCH 03/96] Updated LeftNeutralZoneAuto --- .../frc/robot/Auto/LeftNeutralZoneAuto.java | 91 +++++++++++++++++-- 1 file changed, 83 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java index bed5a56..78ced19 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java @@ -4,19 +4,94 @@ package frc.robot.Auto; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.SwerveConstants; import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.Subsystems.IntakeSubsystem; public class LeftNeutralZoneAuto extends SequentialCommandGroup { - public LeftNeutralZoneAuto (SwerveSubsystem drive) { - addCommands( - new InstantCommand(() -> drive.drive(0.5,0,0, false), drive), - Commands.waitSeconds(2), - new InstantCommand(() -> drive.drive(0,0,0, false), drive) - ); - } -} + // Tune these values to change how far/fast the auto drives. + private static final double INITIAL_BACKUP_DISTANCE_METERS = 3.6; + // Positive value; the auto will negate it to drive backwards. + private static final double INITIAL_BACKUP_SPEED_MPS = 2.0; + private static final double FORWARD_DISTANCE_METERS = 4.0; + private static final double FORWARD_SPEED_MPS = 2.0; + + public LeftNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake) { + this( + drive, + intake, + INITIAL_BACKUP_DISTANCE_METERS, + INITIAL_BACKUP_SPEED_MPS, + FORWARD_DISTANCE_METERS, + FORWARD_SPEED_MPS); + } + + public LeftNeutralZoneAuto( + SwerveSubsystem drive, + IntakeSubsystem intake, + double driveDistanceMeters, + double driveSpeedMps) { + this(drive, intake, driveDistanceMeters, driveSpeedMps, FORWARD_DISTANCE_METERS, FORWARD_SPEED_MPS); + } + + public LeftNeutralZoneAuto( + SwerveSubsystem drive, + IntakeSubsystem intake, + double backupDistanceMeters, + double backupSpeedMps, + double forwardDistanceMeters, + double forwardSpeedMps) { + final double[] startYawRad = new double[1]; + final Pose2d[] startPose = new Pose2d[1]; + final Pose2d[] startPoseAfterTurn = new Pose2d[1]; + final double backupSpeedMpsClamped = + -MathUtil.clamp(Math.abs(backupSpeedMps), 0.0, SwerveConstants.maxSpeed); + final double forwardSpeedMpsClamped = + MathUtil.clamp(Math.abs(forwardSpeedMps), 0.0, SwerveConstants.maxSpeed); + addCommands( + new InstantCommand(intake::lowerIntake, intake), + + // Drive backwards + Commands.runOnce(() -> startPose[0] = drive.getPose(), drive), + Commands.runEnd( + () -> drive.drive(backupSpeedMpsClamped, 0, 0, false), + () -> drive.drive(0, 0, 0, false), + drive) + .until(() -> drive.getPose().getTranslation().getDistance(startPose[0].getTranslation()) >= backupDistanceMeters) + .withTimeout(Math.abs(backupDistanceMeters / Math.max(0.1, Math.abs(backupSpeedMpsClamped))) + 1.0), + + // Turn 90 degrees left (CCW) relative to current heading. + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = + MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0, 0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + // Drive forward after the turn. + Commands.runOnce(() -> startPoseAfterTurn[0] = drive.getPose(), drive), + Commands.runEnd( + () -> drive.drive(forwardSpeedMpsClamped, 0, 0, false), + () -> drive.drive(0, 0, 0, false), + drive) + .until(() -> drive.getPose() + .getTranslation() + .getDistance(startPoseAfterTurn[0].getTranslation()) >= forwardDistanceMeters) + .withTimeout(Math.abs(forwardDistanceMeters / Math.max(0.1, Math.abs(forwardSpeedMpsClamped))) + 1.0) + ); + } +} From d7a168a95e6ff465f1b22deddf76ccfc8a8c6f82 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 11 Mar 2026 14:39:27 -0500 Subject: [PATCH 04/96] Leftneutralauto&rightneutralauto, some changes to path planner --- .../frc/robot/Auto/LeftNeutralZoneAuto.java | 86 +++++++++++------ .../frc/robot/Auto/RightNeutralZoneAuto.java | 96 +++++++++++++++++++ .../frc/robot/Subsystems/SwerveSubsystem.java | 14 ++- 3 files changed, 166 insertions(+), 30 deletions(-) create mode 100644 src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java index 78ced19..fb6c67e 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants.SwerveConstants; @@ -18,9 +19,12 @@ public class LeftNeutralZoneAuto extends SequentialCommandGroup { // Tune these values to change how far/fast the auto drives. private static final double INITIAL_BACKUP_DISTANCE_METERS = 3.6; // Positive value; the auto will negate it to drive backwards. - private static final double INITIAL_BACKUP_SPEED_MPS = 2.0; + private static final double INITIAL_BACKUP_SPEED_MPS = 3.0; private static final double FORWARD_DISTANCE_METERS = 4.0; - private static final double FORWARD_SPEED_MPS = 2.0; + private static final double FORWARD_SPEED_MPS = 3.0; + + // Intake: 1.0 = full power, 0.0 = off. + private static final double INTAKE_POWER = 1.0; public LeftNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake) { this( @@ -29,7 +33,8 @@ public LeftNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake) { INITIAL_BACKUP_DISTANCE_METERS, INITIAL_BACKUP_SPEED_MPS, FORWARD_DISTANCE_METERS, - FORWARD_SPEED_MPS); + FORWARD_SPEED_MPS, + INTAKE_POWER); } public LeftNeutralZoneAuto( @@ -37,7 +42,10 @@ public LeftNeutralZoneAuto( IntakeSubsystem intake, double driveDistanceMeters, double driveSpeedMps) { - this(drive, intake, driveDistanceMeters, driveSpeedMps, FORWARD_DISTANCE_METERS, FORWARD_SPEED_MPS); + // Preserve the existing overload while letting you set the speed easily: + // - driveDistanceMeters controls the initial backup distance + // - driveSpeedMps is used for both the backup and forward drives + this(drive, intake, driveDistanceMeters, driveSpeedMps, FORWARD_DISTANCE_METERS, driveSpeedMps, INTAKE_POWER); } public LeftNeutralZoneAuto( @@ -46,52 +54,72 @@ public LeftNeutralZoneAuto( double backupDistanceMeters, double backupSpeedMps, double forwardDistanceMeters, - double forwardSpeedMps) { - final double[] startYawRad = new double[1]; - final Pose2d[] startPose = new Pose2d[1]; - final Pose2d[] startPoseAfterTurn = new Pose2d[1]; + double forwardSpeedMps, + double intakePower) { final double backupSpeedMpsClamped = -MathUtil.clamp(Math.abs(backupSpeedMps), 0.0, SwerveConstants.maxSpeed); final double forwardSpeedMpsClamped = MathUtil.clamp(Math.abs(forwardSpeedMps), 0.0, SwerveConstants.maxSpeed); + final double intakePowerClamped = MathUtil.clamp(intakePower, -1.0, 1.0); addCommands( new InstantCommand(intake::lowerIntake, intake), - // Drive backwards + // 1) Drive backwards 3.6m. + driveStraightDistanceMeters(drive, backupSpeedMpsClamped, backupDistanceMeters), + + // 2) Turn 90 degrees left. + turnRelativeDegrees(drive, 90.0), + + // 3) Start intake while driving forward 4m. + Commands.runOnce(() -> intake.setIntakePower(intakePowerClamped), intake), + driveStraightDistanceMeters(drive, forwardSpeedMpsClamped, forwardDistanceMeters), + + // 4) Turn 180 degrees with intake still running. + turnRelativeDegrees(drive, 180.0), + + // 5) Drive forward 4m again (intake still running). + driveStraightDistanceMeters(drive, forwardSpeedMpsClamped, forwardDistanceMeters), + + Commands.runOnce(() -> intake.setIntakePower(0.0), intake) + ); + } + + private static Command driveStraightDistanceMeters(SwerveSubsystem drive, double speedMps, double distanceMeters) { + final Pose2d[] startPose = new Pose2d[1]; + final double timeoutSeconds = + Math.abs(distanceMeters / Math.max(0.1, Math.abs(speedMps))) + 1.0; + + return Commands.sequence( Commands.runOnce(() -> startPose[0] = drive.getPose(), drive), Commands.runEnd( - () -> drive.drive(backupSpeedMpsClamped, 0, 0, false), - () -> drive.drive(0, 0, 0, false), + () -> drive.drive(speedMps, 0.0, 0.0, false), + () -> drive.drive(0.0, 0.0, 0.0, false), drive) - .until(() -> drive.getPose().getTranslation().getDistance(startPose[0].getTranslation()) >= backupDistanceMeters) - .withTimeout(Math.abs(backupDistanceMeters / Math.max(0.1, Math.abs(backupSpeedMpsClamped))) + 1.0), + .until(() -> + drive.getPose().getTranslation().getDistance(startPose[0].getTranslation()) >= distanceMeters) + .withTimeout(timeoutSeconds) + ); + } - // Turn 90 degrees left (CCW) relative to current heading. + private static Command turnRelativeDegrees(SwerveSubsystem drive, double degrees) { + final double[] startYawRad = new double[1]; + final double targetDeltaRad = Math.toRadians(degrees); + + return Commands.sequence( Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), Commands.run(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(90.0); + double targetYawRad = startYawRad[0] + targetDeltaRad; double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); - drive.drive(0, 0, omegaRadiansPerSecond, false); + drive.drive(0.0, 0.0, omegaRadiansPerSecond, false); }, drive).until(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(90.0); + double targetYawRad = startYawRad[0] + targetDeltaRad; double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); return Math.abs(errorRad) < Math.toRadians(3.0); }), - Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), - - // Drive forward after the turn. - Commands.runOnce(() -> startPoseAfterTurn[0] = drive.getPose(), drive), - Commands.runEnd( - () -> drive.drive(forwardSpeedMpsClamped, 0, 0, false), - () -> drive.drive(0, 0, 0, false), - drive) - .until(() -> drive.getPose() - .getTranslation() - .getDistance(startPoseAfterTurn[0].getTranslation()) >= forwardDistanceMeters) - .withTimeout(Math.abs(forwardDistanceMeters / Math.max(0.1, Math.abs(forwardSpeedMpsClamped))) + 1.0) + Commands.runOnce(() -> drive.drive(0.0, 0.0, 0.0, false), drive) ); } } diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java new file mode 100644 index 0000000..4ab8d7b --- /dev/null +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java @@ -0,0 +1,96 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.Subsystems.IntakeSubsystem; + +public class RightNeutralZoneAuto extends SequentialCommandGroup { + // Tune these values to change how far/fast the auto drives. + private static final double INITIAL_BACKUP_DISTANCE_METERS = 3.6; + // Positive value; the auto will negate it to drive backwards. + private static final double INITIAL_BACKUP_SPEED_MPS = 2.0; + private static final double FORWARD_DISTANCE_METERS = 4.0; + private static final double FORWARD_SPEED_MPS = 2.0; + + public RightNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake) { + this( + drive, + intake, + INITIAL_BACKUP_DISTANCE_METERS, + INITIAL_BACKUP_SPEED_MPS, + FORWARD_DISTANCE_METERS, + FORWARD_SPEED_MPS); + } + + public RightNeutralZoneAuto( + SwerveSubsystem drive, + IntakeSubsystem intake, + double driveDistanceMeters, + double driveSpeedMps) { + this(drive, intake, driveDistanceMeters, driveSpeedMps, FORWARD_DISTANCE_METERS, FORWARD_SPEED_MPS); + } + + public RightNeutralZoneAuto( + SwerveSubsystem drive, + IntakeSubsystem intake, + double backupDistanceMeters, + double backupSpeedMps, + double forwardDistanceMeters, + double forwardSpeedMps) { + final double[] startYawRad = new double[1]; + final Pose2d[] startPose = new Pose2d[1]; + final Pose2d[] startPoseAfterTurn = new Pose2d[1]; + final double backupSpeedMpsClamped = + -MathUtil.clamp(Math.abs(backupSpeedMps), 0.0, SwerveConstants.maxSpeed); + final double forwardSpeedMpsClamped = + MathUtil.clamp(Math.abs(forwardSpeedMps), 0.0, SwerveConstants.maxSpeed); + + addCommands( + new InstantCommand(intake::lowerIntake, intake), + + // Drive backwards + Commands.runOnce(() -> startPose[0] = drive.getPose(), drive), + Commands.runEnd( + () -> drive.drive(backupSpeedMpsClamped, 0, 0, false), + () -> drive.drive(0, 0, 0, false), + drive) + .until(() -> drive.getPose().getTranslation().getDistance(startPose[0].getTranslation()) >= backupDistanceMeters) + .withTimeout(Math.abs(backupDistanceMeters / Math.max(0.1, Math.abs(backupSpeedMpsClamped))) + 1.0), + + // Turn 90 degrees left (CCW) relative to current heading. + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = + MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0, 0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + // Drive forward after the turn. + Commands.runOnce(() -> startPoseAfterTurn[0] = drive.getPose(), drive), + Commands.runEnd( + () -> drive.drive(forwardSpeedMpsClamped, 0, 0, false), + () -> drive.drive(0, 0, 0, false), + drive) + .until(() -> drive.getPose() + .getTranslation() + .getDistance(startPoseAfterTurn[0].getTranslation()) >= forwardDistanceMeters) + .withTimeout(Math.abs(forwardDistanceMeters / Math.max(0.1, Math.abs(forwardSpeedMpsClamped))) + 1.0) + ); + } +} diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 27244d3..799c565 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -7,6 +7,9 @@ import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.FollowPathCommand; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.math.VecBuilder; @@ -120,7 +123,16 @@ public Command autoDrive(String filename){ if (AutoConstants.isRightSideAuto()){ path = path.mirrorPath(); } - return AutoBuilder.followPath(path); + return new FollowPathCommand(path, + this::getPose, + this::getChassisSpeeds, + (speeds, feedforwards) -> driveFromChassisSpeeds(speeds, isVisionEnabled()), + new PPHolonomicDriveController( + new PIDConstants(SwerveConstants.driveKP, SwerveConstants.driveKI, SwerveConstants.driveKD), + new PIDConstants(SwerveConstants.driveKP, SwerveConstants.driveKI, SwerveConstants.driveKD)), + Constants.AutoConstants.ROBOT_CONFIG, + Constants.FieldConstants::isRedAlliance, + this); } catch(Exception e){ DriverStation.reportError("PATHPLANNER ERROR" + e.getMessage(), e.getStackTrace()); From 2ad2ed8be8ef06dbc2e0665616961f39fec36578 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Wed, 11 Mar 2026 13:54:14 -0600 Subject: [PATCH 05/96] add the auto in constants --- src/main/java/frc/robot/Constants.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b321e90..ba0e175 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -20,6 +20,8 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Auto.LeftNeutralZoneAuto; +import frc.robot.Auto.RightNeutralZoneAuto; /** Central location for robot-wide constants grouped by subsystem and feature */ public final class Constants { @@ -160,6 +162,8 @@ public enum AutoMode{ DriveTestAuto, LeftLemonAuto, RightLemonAuto, + LeftNeutralZoneAuto, + RightNeutralZoneAuto, ShootEightAuto, CenterLemonAuto } @@ -181,6 +185,8 @@ public static void initDashboard() { autoModeChooser.addOption("ShootEightAuto", AutoMode.ShootEightAuto); autoModeChooser.addOption("RightLemonAuto", AutoMode.RightLemonAuto); autoModeChooser.addOption("LeftLemonAuto", AutoMode.LeftLemonAuto); + autoModeChooser.addOption("RightNeutralZoneAuto", AutoMode.RightNeutralZoneAuto); + autoModeChooser.addOption("LeftNeutralZoneAuto", AutoMode.LeftNeutralZoneAuto); autoModeChooser.addOption("CenterLemonAuto", AutoMode.CenterLemonAuto); SmartDashboard.putData("Auto Starting Location", sideChooser); From 73eeca6fb3d93a057ddc0e7315faecac6df3637d Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 11 Mar 2026 14:59:31 -0500 Subject: [PATCH 06/96] neutral zone auto --- src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java | 1 + src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java | 1 + src/main/java/frc/robot/RobotContainer.java | 4 ++++ 3 files changed, 6 insertions(+) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java index fb6c67e..5583437 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java @@ -56,6 +56,7 @@ public LeftNeutralZoneAuto( double forwardDistanceMeters, double forwardSpeedMps, double intakePower) { + addRequirements(drive, intake); final double backupSpeedMpsClamped = -MathUtil.clamp(Math.abs(backupSpeedMps), 0.0, SwerveConstants.maxSpeed); final double forwardSpeedMpsClamped = diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java index 4ab8d7b..389575b 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java @@ -46,6 +46,7 @@ public RightNeutralZoneAuto( double backupSpeedMps, double forwardDistanceMeters, double forwardSpeedMps) { + addRequirements(drive, intake); final double[] startYawRad = new double[1]; final Pose2d[] startPose = new Pose2d[1]; final Pose2d[] startPoseAfterTurn = new Pose2d[1]; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7c0b076..6a9bace 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,7 +21,9 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Auto.LeftLemonAuto; +import frc.robot.Auto.LeftNeutralZoneAuto; import frc.robot.Auto.RightLemonAuto; +import frc.robot.Auto.RightNeutralZoneAuto; import frc.robot.Auto.ShootEightAuto; import frc.robot.Auto.CenterLemonAuto; import frc.robot.Constants.AutoConstants; @@ -235,6 +237,8 @@ public Command getAutonomousCommand() { case None -> Commands.none(); case LeftLemonAuto -> new LeftLemonAuto(m_drive, m_intake, m_shooter); case RightLemonAuto -> new RightLemonAuto(m_drive, m_intake, m_shooter); + case LeftNeutralZoneAuto -> new LeftNeutralZoneAuto(m_drive, m_intake); + case RightNeutralZoneAuto -> new RightNeutralZoneAuto(m_drive, m_intake); case ShootEightAuto -> new ShootEightAuto(m_drive, m_intake, m_shooter); case CenterLemonAuto -> new CenterLemonAuto(m_drive, m_intake, m_shooter); From 6dac2012b74214f2d9d0d962197b1380df2c30d3 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 11 Mar 2026 15:20:26 -0500 Subject: [PATCH 07/96] auto --- src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java | 4 ++-- src/main/java/frc/robot/Constants.java | 5 +++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java index 5583437..d817534 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java @@ -19,9 +19,9 @@ public class LeftNeutralZoneAuto extends SequentialCommandGroup { // Tune these values to change how far/fast the auto drives. private static final double INITIAL_BACKUP_DISTANCE_METERS = 3.6; // Positive value; the auto will negate it to drive backwards. - private static final double INITIAL_BACKUP_SPEED_MPS = 3.0; + private static final double INITIAL_BACKUP_SPEED_MPS = 2.0; private static final double FORWARD_DISTANCE_METERS = 4.0; - private static final double FORWARD_SPEED_MPS = 3.0; + private static final double FORWARD_SPEED_MPS = 2.0; // Intake: 1.0 = full power, 0.0 = off. private static final double INTAKE_POWER = 1.0; diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ba0e175..bf9c7cb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -315,7 +315,8 @@ public static final class IntakeConstants { // Must be unique across *all* CAN devices (SparkMax/SparkFlex/etc). // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. public static int INTAKE_ID = 19; - public static double INTAKE_SPEED = 90; //percent output scaling for intake motor + // SparkMax.set(...) expects [-1.0, 1.0] percent output. + public static double INTAKE_SPEED = 0.90; // max percent output for intake motor public static int INTAKE_ARM_ID = 18; public static int GEAR_RATIO = 25; @@ -344,4 +345,4 @@ public static final class CANdleConstants { public static final int CANDLE_ID = 18; //Placeholder ID } -} \ No newline at end of file +} From 846a68dc4124fc0ad8c64699ed40b79b837df3fe Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 11 Mar 2026 15:31:49 -0500 Subject: [PATCH 08/96] intake for auto --- src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java index d817534..60357b5 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java @@ -24,7 +24,8 @@ public class LeftNeutralZoneAuto extends SequentialCommandGroup { private static final double FORWARD_SPEED_MPS = 2.0; // Intake: 1.0 = full power, 0.0 = off. - private static final double INTAKE_POWER = 1.0; + // Note: On this robot, negative power pulls game pieces in (intake). + private static final double INTAKE_POWER = -1.0; public LeftNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake) { this( From 03b1378ad0a549baca717006ac007d745a859c15 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 11 Mar 2026 16:00:49 -0500 Subject: [PATCH 09/96] left neutral zone auto --- .../frc/robot/Auto/LeftNeutralZoneAuto.java | 20 +++++++++++++++++-- src/main/java/frc/robot/Robot.java | 2 ++ src/main/java/frc/robot/RobotContainer.java | 4 +++- 3 files changed, 23 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java index 60357b5..b07645c 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java @@ -27,6 +27,8 @@ public class LeftNeutralZoneAuto extends SequentialCommandGroup { // Note: On this robot, negative power pulls game pieces in (intake). private static final double INTAKE_POWER = -1.0; + private static final double STRAIGHT_HEADING_KP = 4.0; + public LeftNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake) { this( drive, @@ -66,6 +68,8 @@ public LeftNeutralZoneAuto( addCommands( new InstantCommand(intake::lowerIntake, intake), + Commands.runOnce(() -> drive.drive(0.0, 0.0, 0.0, false), drive), + Commands.waitSeconds(0.1), // 1) Drive backwards 3.6m. driveStraightDistanceMeters(drive, backupSpeedMpsClamped, backupDistanceMeters), @@ -89,13 +93,25 @@ public LeftNeutralZoneAuto( private static Command driveStraightDistanceMeters(SwerveSubsystem drive, double speedMps, double distanceMeters) { final Pose2d[] startPose = new Pose2d[1]; + final double[] holdYawRad = new double[1]; final double timeoutSeconds = Math.abs(distanceMeters / Math.max(0.1, Math.abs(speedMps))) + 1.0; return Commands.sequence( - Commands.runOnce(() -> startPose[0] = drive.getPose(), drive), + Commands.runOnce(() -> { + startPose[0] = drive.getPose(); + holdYawRad[0] = drive.getYaw().getRadians(); + }, drive), Commands.runEnd( - () -> drive.drive(speedMps, 0.0, 0.0, false), + () -> { + double errorRad = MathUtil.angleModulus(holdYawRad[0] - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = + MathUtil.clamp( + errorRad * STRAIGHT_HEADING_KP, + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + drive.drive(speedMps, 0.0, omegaRadiansPerSecond, false); + }, () -> drive.drive(0.0, 0.0, 0.0, false), drive) .until(() -> diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b4ec70d..055232d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.Constants.AutoConstants; public class Robot extends TimedRobot { @@ -23,6 +24,7 @@ public Robot() { @Override public void robotInit() { + AutoConstants.initDashboard(); CameraServer.startAutomaticCapture(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6a9bace..7c03be2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,6 +36,7 @@ import frc.robot.Subsystems.ShooterSubsystem; import frc.robot.Subsystems.SwerveSubsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.DriverStation; public class RobotContainer { @@ -71,7 +72,6 @@ public class RobotContainer { private UsbCamera driverCamera; public RobotContainer() { - AutoConstants.initDashboard(); startLimelightStreams(); startDriverCameraStream(); configureBindings(); @@ -232,6 +232,8 @@ private double getSpeedMultiplier(){ public Command getAutonomousCommand() { AutoConstants.AutoMode selected = AutoConstants.getSelectedAutoMode(); + SmartDashboard.putString("Auto/Selected", selected.name()); + DriverStation.reportWarning("Auto selected: " + selected.name(), false); return switch (selected) { case None -> Commands.none(); From 2f6828377b0041bdfadd984f7e8569a88d6571bb Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 11 Mar 2026 16:52:28 -0500 Subject: [PATCH 10/96] Neutral auto --- .../frc/robot/Auto/LeftNeutralZoneAuto.java | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java index b07645c..157101c 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java @@ -23,8 +23,8 @@ public class LeftNeutralZoneAuto extends SequentialCommandGroup { private static final double FORWARD_DISTANCE_METERS = 4.0; private static final double FORWARD_SPEED_MPS = 2.0; - // Intake: 1.0 = full power, 0.0 = off. - // Note: On this robot, negative power pulls game pieces in (intake). + // Intake: 1.0 = full power + // Negative power pulls game pieces in (intake) for Pesto private static final double INTAKE_POWER = -1.0; private static final double STRAIGHT_HEADING_KP = 4.0; @@ -81,11 +81,17 @@ public LeftNeutralZoneAuto( Commands.runOnce(() -> intake.setIntakePower(intakePowerClamped), intake), driveStraightDistanceMeters(drive, forwardSpeedMpsClamped, forwardDistanceMeters), - // 4) Turn 180 degrees with intake still running. - turnRelativeDegrees(drive, 180.0), + // 4) Turn 90 degrees right with intake still running. + turnRelativeDegrees(drive, -90.0), - // 5) Drive forward 4m again (intake still running). - driveStraightDistanceMeters(drive, forwardSpeedMpsClamped, forwardDistanceMeters), + // 5) Drive forward 1m (intake still running). + driveStraightDistanceMeters(drive, forwardSpeedMpsClamped, 1.0), + + // 6) Turn 90 degrees right (intake still running). + turnRelativeDegrees(drive, -90.0), + + // 7) Drive forward 3m (intake still running). + driveStraightDistanceMeters(drive, forwardSpeedMpsClamped, 3.0), Commands.runOnce(() -> intake.setIntakePower(0.0), intake) ); From 4fa415471f34debf634fe844a299f4079ae2ae9f Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 11 Mar 2026 18:28:30 -0500 Subject: [PATCH 11/96] left neutral zone auto --- .../frc/robot/Auto/LeftNeutralZoneAuto.java | 162 +++++++----------- 1 file changed, 65 insertions(+), 97 deletions(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java index 157101c..5296f39 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java @@ -7,143 +7,111 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants.SwerveConstants; import frc.robot.Subsystems.SwerveSubsystem; import frc.robot.Subsystems.IntakeSubsystem; +import java.util.concurrent.atomic.AtomicReference; public class LeftNeutralZoneAuto extends SequentialCommandGroup { - // Tune these values to change how far/fast the auto drives. - private static final double INITIAL_BACKUP_DISTANCE_METERS = 3.6; - // Positive value; the auto will negate it to drive backwards. - private static final double INITIAL_BACKUP_SPEED_MPS = 2.0; - private static final double FORWARD_DISTANCE_METERS = 4.0; - private static final double FORWARD_SPEED_MPS = 2.0; + // Tunables (requested). + private static final double DRIVE_SPEED_MPS = 3.0; + private static final double TURN_P = 4.0; + private static final double TURN_TOLERANCE_DEG = 3.0; + private static final double TURN_TIMEOUT_SEC = 2.5; - // Intake: 1.0 = full power - // Negative power pulls game pieces in (intake) for Pesto - private static final double INTAKE_POWER = -1.0; + private static final double BACKWARD_METERS_1 = 3.4; + private static final double FORWARD_METERS_1 = 3.0; + private static final double FORWARD_METERS_2 = 1.0; + private static final double FORWARD_METERS_3 = 3.0; - private static final double STRAIGHT_HEADING_KP = 4.0; + private static final double INTAKE_POWER = 1.0; public LeftNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake) { - this( - drive, - intake, - INITIAL_BACKUP_DISTANCE_METERS, - INITIAL_BACKUP_SPEED_MPS, - FORWARD_DISTANCE_METERS, - FORWARD_SPEED_MPS, - INTAKE_POWER); - } - - public LeftNeutralZoneAuto( - SwerveSubsystem drive, - IntakeSubsystem intake, - double driveDistanceMeters, - double driveSpeedMps) { - // Preserve the existing overload while letting you set the speed easily: - // - driveDistanceMeters controls the initial backup distance - // - driveSpeedMps is used for both the backup and forward drives - this(drive, intake, driveDistanceMeters, driveSpeedMps, FORWARD_DISTANCE_METERS, driveSpeedMps, INTAKE_POWER); - } - - public LeftNeutralZoneAuto( - SwerveSubsystem drive, - IntakeSubsystem intake, - double backupDistanceMeters, - double backupSpeedMps, - double forwardDistanceMeters, - double forwardSpeedMps, - double intakePower) { addRequirements(drive, intake); - final double backupSpeedMpsClamped = - -MathUtil.clamp(Math.abs(backupSpeedMps), 0.0, SwerveConstants.maxSpeed); - final double forwardSpeedMpsClamped = - MathUtil.clamp(Math.abs(forwardSpeedMps), 0.0, SwerveConstants.maxSpeed); - final double intakePowerClamped = MathUtil.clamp(intakePower, -1.0, 1.0); addCommands( - new InstantCommand(intake::lowerIntake, intake), - Commands.runOnce(() -> drive.drive(0.0, 0.0, 0.0, false), drive), - Commands.waitSeconds(0.1), + Commands.runOnce(intake::lowerIntake, intake), - // 1) Drive backwards 3.6m. - driveStraightDistanceMeters(drive, backupSpeedMpsClamped, backupDistanceMeters), + // 1) Drive backwards 3.4m. + driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS), // 2) Turn 90 degrees left. turnRelativeDegrees(drive, 90.0), - // 3) Start intake while driving forward 4m. - Commands.runOnce(() -> intake.setIntakePower(intakePowerClamped), intake), - driveStraightDistanceMeters(drive, forwardSpeedMpsClamped, forwardDistanceMeters), + // 3) Drive forward 3m while starting intake (intake stays on for the rest of auto). + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), - // 4) Turn 90 degrees right with intake still running. + // 4) Turn 90 degrees right (intake still on). turnRelativeDegrees(drive, -90.0), - // 5) Drive forward 1m (intake still running). - driveStraightDistanceMeters(drive, forwardSpeedMpsClamped, 1.0), + // 5) Drive forward 1m (intake still on). + driveDistanceMeters(drive, FORWARD_METERS_2, DRIVE_SPEED_MPS), - // 6) Turn 90 degrees right (intake still running). + // 6) Turn 90 degrees right (intake still on). turnRelativeDegrees(drive, -90.0), - // 7) Drive forward 3m (intake still running). - driveStraightDistanceMeters(drive, forwardSpeedMpsClamped, 3.0), + // 7) Drive forward 3m (intake still on). + driveDistanceMeters(drive, FORWARD_METERS_3, DRIVE_SPEED_MPS), - Commands.runOnce(() -> intake.setIntakePower(0.0), intake) + // Stop intake at the end. + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive) ); } - private static Command driveStraightDistanceMeters(SwerveSubsystem drive, double speedMps, double distanceMeters) { - final Pose2d[] startPose = new Pose2d[1]; - final double[] holdYawRad = new double[1]; - final double timeoutSeconds = - Math.abs(distanceMeters / Math.max(0.1, Math.abs(speedMps))) + 1.0; + private static edu.wpi.first.wpilibj2.command.Command driveDistanceMeters( + SwerveSubsystem drive, + double distanceMeters, + double speedMps) { + double clampedSpeedMps = MathUtil.clamp(Math.abs(speedMps), 0.0, SwerveConstants.maxSpeed); + double commandedSpeedMps = Math.copySign(clampedSpeedMps, distanceMeters); + double distanceAbsMeters = Math.abs(distanceMeters); + + AtomicReference startPose = new AtomicReference<>(new Pose2d()); return Commands.sequence( - Commands.runOnce(() -> { - startPose[0] = drive.getPose(); - holdYawRad[0] = drive.getYaw().getRadians(); - }, drive), + Commands.runOnce(() -> startPose.set(drive.getPose()), drive), Commands.runEnd( - () -> { - double errorRad = MathUtil.angleModulus(holdYawRad[0] - drive.getYaw().getRadians()); - double omegaRadiansPerSecond = - MathUtil.clamp( - errorRad * STRAIGHT_HEADING_KP, - -SwerveConstants.maxAngularVelocity, - SwerveConstants.maxAngularVelocity); - drive.drive(speedMps, 0.0, omegaRadiansPerSecond, false); - }, - () -> drive.drive(0.0, 0.0, 0.0, false), + () -> drive.drive(commandedSpeedMps, 0, 0, false), + () -> drive.drive(0, 0, 0, false), drive) - .until(() -> - drive.getPose().getTranslation().getDistance(startPose[0].getTranslation()) >= distanceMeters) - .withTimeout(timeoutSeconds) + .until( + () -> + drive.getPose().getTranslation().getDistance(startPose.get().getTranslation()) + >= distanceAbsMeters) + .withTimeout(distanceAbsMeters / Math.max(0.1, Math.abs(commandedSpeedMps)) + 1.0) ); } - private static Command turnRelativeDegrees(SwerveSubsystem drive, double degrees) { + private static edu.wpi.first.wpilibj2.command.Command turnRelativeDegrees( + SwerveSubsystem drive, + double deltaDegrees) { final double[] startYawRad = new double[1]; - final double targetDeltaRad = Math.toRadians(degrees); return Commands.sequence( Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), - Commands.run(() -> { - double targetYawRad = startYawRad[0] + targetDeltaRad; - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - double omegaRadiansPerSecond = - MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); - drive.drive(0.0, 0.0, omegaRadiansPerSecond, false); - }, drive).until(() -> { - double targetYawRad = startYawRad[0] + targetDeltaRad; - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - return Math.abs(errorRad) < Math.toRadians(3.0); - }), - Commands.runOnce(() -> drive.drive(0.0, 0.0, 0.0, false), drive) + Commands.runEnd( + () -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadPerSec = + MathUtil.clamp( + errorRad * TURN_P, + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + drive.drive(0, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(TURN_TOLERANCE_DEG); + }) + .withTimeout(TURN_TIMEOUT_SEC) ); } } From e7a38c5e71cdb865bd79145b041d853c432f6012 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 11 Mar 2026 19:15:23 -0500 Subject: [PATCH 12/96] neutral zone with pathplanner --- .../deploy/pathplanner/paths/Neutral.path | 54 +++++++++++++++++++ src/main/deploy/pathplanner/paths/Trench.path | 54 +++++++++++++++++++ .../java/frc/robot/Auto/DriveTestAuto.java | 5 +- 3 files changed, 111 insertions(+), 2 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Neutral.path create mode 100644 src/main/deploy/pathplanner/paths/Trench.path diff --git a/src/main/deploy/pathplanner/paths/Neutral.path b/src/main/deploy/pathplanner/paths/Neutral.path new file mode 100644 index 0000000..4f95595 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Neutral.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.443, + "y": 7.434 + }, + "prevControl": null, + "nextControl": { + "x": 7.433072048611112, + "y": 6.4517639612268525 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.443, + "y": 4.440079427083334 + }, + "prevControl": { + "x": 7.4900876012731485, + "y": 5.080503833912038 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": -90.96353558963717 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -89.64693045105892 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Trench.path b/src/main/deploy/pathplanner/paths/Trench.path new file mode 100644 index 0000000..f538b22 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Trench.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5615257523148154, + "y": 7.306603587962964 + }, + "prevControl": null, + "nextControl": { + "x": 4.336320529513889, + "y": 7.318282262731482 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.733961950231482, + "y": 7.306603587962964 + }, + "prevControl": { + "x": 6.733961950231482, + "y": 7.306603587962964 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": -90.37981946249927 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -179.6195218436119 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Auto/DriveTestAuto.java b/src/main/java/frc/robot/Auto/DriveTestAuto.java index 6975029..f31d5e6 100644 --- a/src/main/java/frc/robot/Auto/DriveTestAuto.java +++ b/src/main/java/frc/robot/Auto/DriveTestAuto.java @@ -23,8 +23,9 @@ public DriveTestAuto (SwerveSubsystem drive) { public class DriveTestAuto extends SequentialCommandGroup { public DriveTestAuto (SwerveSubsystem drive){ addCommands( - drive.startAutoAt(1.165, 6.000, 0.000), - drive.autoDrive("DriveTestPath") + drive.startAutoAt(3.628, 7.434, 0.864), + drive.autoDrive("Trench"), + drive.autoDrive("Neutral") ); } } From 9e86162a1e14b3d40793d3605b649a246fd1b87f Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Wed, 11 Mar 2026 18:25:23 -0600 Subject: [PATCH 13/96] auto --- src/main/java/frc/robot/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7c03be2..fdd94d2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,7 @@ import frc.robot.Auto.RightNeutralZoneAuto; import frc.robot.Auto.ShootEightAuto; import frc.robot.Auto.CenterLemonAuto; +import frc.robot.Auto.DriveTestAuto; import frc.robot.Constants.AutoConstants; import frc.robot.Constants.VisionConstants; import frc.robot.Constants.ShooterConstants; @@ -237,6 +238,7 @@ public Command getAutonomousCommand() { return switch (selected) { case None -> Commands.none(); + case DriveTestAuto -> new DriveTestAuto(m_drive); case LeftLemonAuto -> new LeftLemonAuto(m_drive, m_intake, m_shooter); case RightLemonAuto -> new RightLemonAuto(m_drive, m_intake, m_shooter); case LeftNeutralZoneAuto -> new LeftNeutralZoneAuto(m_drive, m_intake); From 90d54f543ba14fa8e6512707a481562c5925a138 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 11 Mar 2026 19:27:00 -0500 Subject: [PATCH 14/96] auto trench path --- src/main/deploy/pathplanner/paths/Trench.path | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/paths/Trench.path b/src/main/deploy/pathplanner/paths/Trench.path index f538b22..df58398 100644 --- a/src/main/deploy/pathplanner/paths/Trench.path +++ b/src/main/deploy/pathplanner/paths/Trench.path @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -179.6195218436119 + "rotation": -3.0426375790403157 }, "useDefaultConstraints": true } \ No newline at end of file From 4698853e65d970ba0d8f32079e27d70fb0d53498 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Thu, 12 Mar 2026 14:11:06 -0500 Subject: [PATCH 15/96] pathplanner auto --- .../deploy/pathplanner/paths/Neutral.path | 16 +-- .../deploy/pathplanner/paths/Neutral2.path | 54 +++++++++++ .../deploy/pathplanner/paths/Neutral3.path | 54 +++++++++++ .../deploy/pathplanner/paths/Neutral4.path | 54 +++++++++++ src/main/deploy/pathplanner/paths/Trench.path | 2 +- .../java/frc/robot/Auto/DriveTestAuto.java | 49 +++++++--- .../frc/robot/Auto/LeftNeutralZoneAuto.java | 15 ++- .../frc/robot/Auto/RightNeutralZoneAuto.java | 97 ------------------- src/main/java/frc/robot/Constants.java | 1 - src/main/java/frc/robot/RobotContainer.java | 4 +- 10 files changed, 212 insertions(+), 134 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Neutral2.path create mode 100644 src/main/deploy/pathplanner/paths/Neutral3.path create mode 100644 src/main/deploy/pathplanner/paths/Neutral4.path delete mode 100644 src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java diff --git a/src/main/deploy/pathplanner/paths/Neutral.path b/src/main/deploy/pathplanner/paths/Neutral.path index 4f95595..6033ecd 100644 --- a/src/main/deploy/pathplanner/paths/Neutral.path +++ b/src/main/deploy/pathplanner/paths/Neutral.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.443, - "y": 7.434 + "x": 7.734, + "y": 7.416 }, "prevControl": null, "nextControl": { - "x": 7.433072048611112, - "y": 6.4517639612268525 + "x": 7.724072048611112, + "y": 6.433763961226853 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.443, - "y": 4.440079427083334 + "x": 7.734, + "y": 4.550436342592593 }, "prevControl": { - "x": 7.4900876012731485, - "y": 5.080503833912038 + "x": 7.781087601273149, + "y": 5.190860749421297 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Neutral2.path b/src/main/deploy/pathplanner/paths/Neutral2.path new file mode 100644 index 0000000..56205c1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Neutral2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.734, + "y": 4.550436342592593 + }, + "prevControl": null, + "nextControl": { + "x": 6.758923828125001, + "y": 4.543087962962963 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.389864583333334, + "y": 4.550436342592593 + }, + "prevControl": { + "x": 7.1843425202546305, + "y": 4.545974826388889 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": -90.96353558963717 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -91.28065898863989 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Neutral3.path b/src/main/deploy/pathplanner/paths/Neutral3.path new file mode 100644 index 0000000..f7820a6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Neutral3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.39, + "y": 4.550436342592593 + }, + "prevControl": null, + "nextControl": { + "x": 6.148155381944445, + "y": 5.488601345486111 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.622352575231482, + "y": 7.352006076388888 + }, + "prevControl": { + "x": 5.813935329861112, + "y": 6.452879340277778 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": 89.85355743195251 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 88.65080525169873 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Neutral4.path b/src/main/deploy/pathplanner/paths/Neutral4.path new file mode 100644 index 0000000..d2d7dc6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Neutral4.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.622, + "y": 7.352 + }, + "prevControl": null, + "nextControl": { + "x": 4.729130787037037, + "y": 7.329042390046296 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.4963744936342596, + "y": 7.246963614004629 + }, + "prevControl": { + "x": 4.508482566550927, + "y": 7.295712239583334 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": 28.352937229598172 + }, + "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/Trench.path b/src/main/deploy/pathplanner/paths/Trench.path index df58398..026c096 100644 --- a/src/main/deploy/pathplanner/paths/Trench.path +++ b/src/main/deploy/pathplanner/paths/Trench.path @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -3.0426375790403157 + "rotation": 0.6234506722048545 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc/robot/Auto/DriveTestAuto.java b/src/main/java/frc/robot/Auto/DriveTestAuto.java index f31d5e6..e8dc273 100644 --- a/src/main/java/frc/robot/Auto/DriveTestAuto.java +++ b/src/main/java/frc/robot/Auto/DriveTestAuto.java @@ -4,29 +4,46 @@ package frc.robot.Auto; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; import frc.robot.Subsystems.SwerveSubsystem; -/* public class DriveTestAuto extends SequentialCommandGroup { - public DriveTestAuto (SwerveSubsystem drive) { - addCommands( - new InstantCommand(() -> drive.drive(0.5,0,0, false), drive), - Commands.waitSeconds(2), - new InstantCommand(() -> drive.drive(0,0,0, false), drive) - ); - } -} -*/ - + public DriveTestAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + addRequirements(drive, intake, shooter); -public class DriveTestAuto extends SequentialCommandGroup { - public DriveTestAuto (SwerveSubsystem drive){ addCommands( - drive.startAutoAt(3.628, 7.434, 0.864), + drive.startAutoAt(3.562, 7.307, 0.864), drive.autoDrive("Trench"), - drive.autoDrive("Neutral") + Commands.deadline( + Commands.sequence( + drive.autoDrive("Neutral"), + drive.autoDrive("Neutral2"), + drive.autoDrive("Neutral3") + ), + Commands.startEnd( + () -> intake.setIntakePower(1.0), + () -> intake.setIntakePower(0.0), + intake) + ), + drive.autoDrive("Neutral4"), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + Commands.sequence( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ) ); } } - diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java index 5296f39..cd8eebf 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java @@ -15,7 +15,6 @@ import java.util.concurrent.atomic.AtomicReference; public class LeftNeutralZoneAuto extends SequentialCommandGroup { - // Tunables (requested). private static final double DRIVE_SPEED_MPS = 3.0; private static final double TURN_P = 4.0; private static final double TURN_TOLERANCE_DEG = 3.0; @@ -34,26 +33,26 @@ public LeftNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake) { addCommands( Commands.runOnce(intake::lowerIntake, intake), - // 1) Drive backwards 3.4m. + // Drive backwards 3.4m. driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS), - // 2) Turn 90 degrees left. + // Turn 90 degrees left. turnRelativeDegrees(drive, 90.0), - // 3) Drive forward 3m while starting intake (intake stays on for the rest of auto). + // Drive forward 3m while starting intake (intake stays on for the rest of auto). Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), - // 4) Turn 90 degrees right (intake still on). + // Turn 90 degrees right (intake still on). turnRelativeDegrees(drive, -90.0), - // 5) Drive forward 1m (intake still on). + // Drive forward 1m (intake still on). driveDistanceMeters(drive, FORWARD_METERS_2, DRIVE_SPEED_MPS), - // 6) Turn 90 degrees right (intake still on). + // Turn 90 degrees right (intake still on). turnRelativeDegrees(drive, -90.0), - // 7) Drive forward 3m (intake still on). + // Drive forward 3m (intake still on). driveDistanceMeters(drive, FORWARD_METERS_3, DRIVE_SPEED_MPS), // Stop intake at the end. diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java deleted file mode 100644 index 389575b..0000000 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.Auto; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants.SwerveConstants; -import frc.robot.Subsystems.SwerveSubsystem; -import frc.robot.Subsystems.IntakeSubsystem; - -public class RightNeutralZoneAuto extends SequentialCommandGroup { - // Tune these values to change how far/fast the auto drives. - private static final double INITIAL_BACKUP_DISTANCE_METERS = 3.6; - // Positive value; the auto will negate it to drive backwards. - private static final double INITIAL_BACKUP_SPEED_MPS = 2.0; - private static final double FORWARD_DISTANCE_METERS = 4.0; - private static final double FORWARD_SPEED_MPS = 2.0; - - public RightNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake) { - this( - drive, - intake, - INITIAL_BACKUP_DISTANCE_METERS, - INITIAL_BACKUP_SPEED_MPS, - FORWARD_DISTANCE_METERS, - FORWARD_SPEED_MPS); - } - - public RightNeutralZoneAuto( - SwerveSubsystem drive, - IntakeSubsystem intake, - double driveDistanceMeters, - double driveSpeedMps) { - this(drive, intake, driveDistanceMeters, driveSpeedMps, FORWARD_DISTANCE_METERS, FORWARD_SPEED_MPS); - } - - public RightNeutralZoneAuto( - SwerveSubsystem drive, - IntakeSubsystem intake, - double backupDistanceMeters, - double backupSpeedMps, - double forwardDistanceMeters, - double forwardSpeedMps) { - addRequirements(drive, intake); - final double[] startYawRad = new double[1]; - final Pose2d[] startPose = new Pose2d[1]; - final Pose2d[] startPoseAfterTurn = new Pose2d[1]; - final double backupSpeedMpsClamped = - -MathUtil.clamp(Math.abs(backupSpeedMps), 0.0, SwerveConstants.maxSpeed); - final double forwardSpeedMpsClamped = - MathUtil.clamp(Math.abs(forwardSpeedMps), 0.0, SwerveConstants.maxSpeed); - - addCommands( - new InstantCommand(intake::lowerIntake, intake), - - // Drive backwards - Commands.runOnce(() -> startPose[0] = drive.getPose(), drive), - Commands.runEnd( - () -> drive.drive(backupSpeedMpsClamped, 0, 0, false), - () -> drive.drive(0, 0, 0, false), - drive) - .until(() -> drive.getPose().getTranslation().getDistance(startPose[0].getTranslation()) >= backupDistanceMeters) - .withTimeout(Math.abs(backupDistanceMeters / Math.max(0.1, Math.abs(backupSpeedMpsClamped))) + 1.0), - - // Turn 90 degrees left (CCW) relative to current heading. - Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), - Commands.run(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(90.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - double omegaRadiansPerSecond = - MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); - drive.drive(0, 0, omegaRadiansPerSecond, false); - }, drive).until(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(90.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - return Math.abs(errorRad) < Math.toRadians(3.0); - }), - Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), - - // Drive forward after the turn. - Commands.runOnce(() -> startPoseAfterTurn[0] = drive.getPose(), drive), - Commands.runEnd( - () -> drive.drive(forwardSpeedMpsClamped, 0, 0, false), - () -> drive.drive(0, 0, 0, false), - drive) - .until(() -> drive.getPose() - .getTranslation() - .getDistance(startPoseAfterTurn[0].getTranslation()) >= forwardDistanceMeters) - .withTimeout(Math.abs(forwardDistanceMeters / Math.max(0.1, Math.abs(forwardSpeedMpsClamped))) + 1.0) - ); - } -} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bf9c7cb..ce84ad8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -21,7 +21,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Auto.LeftNeutralZoneAuto; -import frc.robot.Auto.RightNeutralZoneAuto; /** Central location for robot-wide constants grouped by subsystem and feature */ public final class Constants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fdd94d2..2bc24b0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,7 +23,6 @@ import frc.robot.Auto.LeftLemonAuto; import frc.robot.Auto.LeftNeutralZoneAuto; import frc.robot.Auto.RightLemonAuto; -import frc.robot.Auto.RightNeutralZoneAuto; import frc.robot.Auto.ShootEightAuto; import frc.robot.Auto.CenterLemonAuto; import frc.robot.Auto.DriveTestAuto; @@ -238,11 +237,10 @@ public Command getAutonomousCommand() { return switch (selected) { case None -> Commands.none(); - case DriveTestAuto -> new DriveTestAuto(m_drive); + case DriveTestAuto -> new DriveTestAuto(m_drive, m_intake, m_shooter); case LeftLemonAuto -> new LeftLemonAuto(m_drive, m_intake, m_shooter); case RightLemonAuto -> new RightLemonAuto(m_drive, m_intake, m_shooter); case LeftNeutralZoneAuto -> new LeftNeutralZoneAuto(m_drive, m_intake); - case RightNeutralZoneAuto -> new RightNeutralZoneAuto(m_drive, m_intake); case ShootEightAuto -> new ShootEightAuto(m_drive, m_intake, m_shooter); case CenterLemonAuto -> new CenterLemonAuto(m_drive, m_intake, m_shooter); From db3a88ca1c2fe8691dad369f35de2ca3fd6c61c6 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Thu, 12 Mar 2026 14:25:59 -0500 Subject: [PATCH 16/96] trench auto fix --- src/main/deploy/pathplanner/paths/Trench.path | 2 +- .../deploy/pathplanner/paths/Trench2.path | 54 +++++++++++++++++++ .../java/frc/robot/Auto/DriveTestAuto.java | 1 + 3 files changed, 56 insertions(+), 1 deletion(-) create mode 100644 src/main/deploy/pathplanner/paths/Trench2.path diff --git a/src/main/deploy/pathplanner/paths/Trench.path b/src/main/deploy/pathplanner/paths/Trench.path index 026c096..e40574e 100644 --- a/src/main/deploy/pathplanner/paths/Trench.path +++ b/src/main/deploy/pathplanner/paths/Trench.path @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -90.37981946249927 + "rotation": 0.3798895366251577 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/Trench2.path b/src/main/deploy/pathplanner/paths/Trench2.path new file mode 100644 index 0000000..9b04eed --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Trench2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.733961950231482, + "y": 7.306603587962964 + }, + "prevControl": null, + "nextControl": { + "x": 7.699385199652777, + "y": 6.840703197337962 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.733961950231482, + "y": 7.306603587962964 + }, + "prevControl": { + "x": 7.721233506944445, + "y": 6.896668981481482 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": -90.00827495365614 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.6234506722048545 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Auto/DriveTestAuto.java b/src/main/java/frc/robot/Auto/DriveTestAuto.java index e8dc273..ec4cebd 100644 --- a/src/main/java/frc/robot/Auto/DriveTestAuto.java +++ b/src/main/java/frc/robot/Auto/DriveTestAuto.java @@ -18,6 +18,7 @@ public DriveTestAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsy addCommands( drive.startAutoAt(3.562, 7.307, 0.864), drive.autoDrive("Trench"), + drive.autoDrive("Trench2"), Commands.deadline( Commands.sequence( drive.autoDrive("Neutral"), From 9ae23d30336582fdfab77a0dfc87717b9cb63075 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Thu, 12 Mar 2026 14:49:31 -0600 Subject: [PATCH 17/96] left neutral zone auto fix --- .../java/frc/robot/Auto/LeftNeutralZoneAuto.java | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java index cd8eebf..7a6192d 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java @@ -15,7 +15,7 @@ import java.util.concurrent.atomic.AtomicReference; public class LeftNeutralZoneAuto extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 3.0; + private static final double DRIVE_SPEED_MPS = 1.0; private static final double TURN_P = 4.0; private static final double TURN_TOLERANCE_DEG = 3.0; private static final double TURN_TIMEOUT_SEC = 2.5; @@ -23,9 +23,10 @@ public class LeftNeutralZoneAuto extends SequentialCommandGroup { private static final double BACKWARD_METERS_1 = 3.4; private static final double FORWARD_METERS_1 = 3.0; private static final double FORWARD_METERS_2 = 1.0; - private static final double FORWARD_METERS_3 = 3.0; + private static final double FORWARD_METERS_3 = 3.1; + private static final double FORWARD_METERS_4 = 1.0; - private static final double INTAKE_POWER = 1.0; + private static final double INTAKE_POWER = -1.0; public LeftNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake) { addRequirements(drive, intake); @@ -57,7 +58,13 @@ public LeftNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake) { // Stop intake at the end. Commands.runOnce(() -> intake.setIntakePower(0.0), intake), - Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive) + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 90 degrees left + turnRelativeDegrees(drive, 90.0), + + // Drive forward (back to the trench) + driveDistanceMeters(drive, FORWARD_METERS_4, DRIVE_SPEED_MPS) ); } From 1e940356a41507404ebe2be138f9b94dc974e14e Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Thu, 12 Mar 2026 15:57:01 -0500 Subject: [PATCH 18/96] use the shooter too in the auto --- .../frc/robot/Auto/LeftNeutralZoneAuto.java | 26 ++++++++++++++++--- src/main/java/frc/robot/RobotContainer.java | 2 +- 2 files changed, 24 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java index 7a6192d..3605d03 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java @@ -8,9 +8,11 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.Subsystems.SwerveSubsystem; import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; import java.util.concurrent.atomic.AtomicReference; @@ -28,8 +30,8 @@ public class LeftNeutralZoneAuto extends SequentialCommandGroup { private static final double INTAKE_POWER = -1.0; - public LeftNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake) { - addRequirements(drive, intake); + public LeftNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + addRequirements(drive, intake, shooter); addCommands( Commands.runOnce(intake::lowerIntake, intake), @@ -64,7 +66,25 @@ public LeftNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake) { turnRelativeDegrees(drive, 90.0), // Drive forward (back to the trench) - driveDistanceMeters(drive, FORWARD_METERS_4, DRIVE_SPEED_MPS) + driveDistanceMeters(drive, FORWARD_METERS_4, DRIVE_SPEED_MPS), + + // Turn 10 degrees left + turnRelativeDegrees(drive, 10.0), + + // Bring hood up to HIGH angle. + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ) ); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2bc24b0..c8ce4b7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -240,7 +240,7 @@ public Command getAutonomousCommand() { case DriveTestAuto -> new DriveTestAuto(m_drive, m_intake, m_shooter); case LeftLemonAuto -> new LeftLemonAuto(m_drive, m_intake, m_shooter); case RightLemonAuto -> new RightLemonAuto(m_drive, m_intake, m_shooter); - case LeftNeutralZoneAuto -> new LeftNeutralZoneAuto(m_drive, m_intake); + case LeftNeutralZoneAuto -> new LeftNeutralZoneAuto(m_drive, m_intake, m_shooter); case ShootEightAuto -> new ShootEightAuto(m_drive, m_intake, m_shooter); case CenterLemonAuto -> new CenterLemonAuto(m_drive, m_intake, m_shooter); From 1db00c340041c57d296bccf6386b53f514401eda Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Fri, 13 Mar 2026 15:13:36 -0600 Subject: [PATCH 19/96] value changes on left neutral zone auto --- src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java index 3605d03..280fec5 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java @@ -23,10 +23,10 @@ public class LeftNeutralZoneAuto extends SequentialCommandGroup { private static final double TURN_TIMEOUT_SEC = 2.5; private static final double BACKWARD_METERS_1 = 3.4; + private static final double BACKWARD_METERS_2 = 3.0; private static final double FORWARD_METERS_1 = 3.0; private static final double FORWARD_METERS_2 = 1.0; - private static final double FORWARD_METERS_3 = 3.1; - private static final double FORWARD_METERS_4 = 1.0; + private static final double FORWARD_METERS_3 = 3.3; private static final double INTAKE_POWER = -1.0; @@ -62,11 +62,11 @@ public LeftNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake, Shoote Commands.runOnce(() -> intake.setIntakePower(0.0), intake), Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), - // Turn 90 degrees left - turnRelativeDegrees(drive, 90.0), + // Turn 90 degrees right + turnRelativeDegrees(drive, -100.0), // Drive forward (back to the trench) - driveDistanceMeters(drive, FORWARD_METERS_4, DRIVE_SPEED_MPS), + driveDistanceMeters(drive, -BACKWARD_METERS_2, DRIVE_SPEED_MPS), // Turn 10 degrees left turnRelativeDegrees(drive, 10.0), From 4ab8dbeedf95b545e9a610f54ff276a4dab94985 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Fri, 13 Mar 2026 16:56:53 -0500 Subject: [PATCH 20/96] Changed settings for the robot in pathplanner --- src/main/deploy/pathplanner/settings.json | 32 +++++++++---------- .../frc/robot/Auto/LeftNeutralZoneAuto.java | 31 ++++++++++++++---- src/main/java/frc/robot/Constants.java | 3 -- 3 files changed, 41 insertions(+), 25 deletions(-) diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 145ffd7..1de7b1b 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.9, - "robotLength": 0.9, + "robotWidth": 0.838, + "robotLength": 0.838, "holonomicMode": true, "pathFolders": [ "Offset Depot" @@ -11,23 +11,23 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, - "robotMOI": 6.883, + "robotMass": 49.895, + "robotMOI": 3.9111, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, - "driveGearing": 5.143, + "driveWheelRadius": 0.0508, + "driveGearing": 6.75, "maxDriveSpeed": 5.45, - "driveMotorType": "krakenX60", - "driveCurrentLimit": 60.0, + "driveMotorType": "vortex", + "driveCurrentLimit": 40.0, "wheelCOF": 1.2, - "flModuleX": 0.273, - "flModuleY": 0.273, - "frModuleX": 0.273, - "frModuleY": -0.273, - "blModuleX": -0.273, - "blModuleY": 0.273, - "brModuleX": -0.273, - "brModuleY": -0.273, + "flModuleX": 0.276, + "flModuleY": 0.276, + "frModuleX": 0.276, + "frModuleY": -0.276, + "blModuleX": -0.276, + "blModuleY": 0.276, + "brModuleX": -0.276, + "brModuleY": -0.276, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java index 280fec5..23a00d9 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java @@ -5,7 +5,7 @@ package frc.robot.Auto; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants.ShooterConstants; @@ -16,6 +16,7 @@ import java.util.concurrent.atomic.AtomicReference; + public class LeftNeutralZoneAuto extends SequentialCommandGroup { private static final double DRIVE_SPEED_MPS = 1.0; private static final double TURN_P = 4.0; @@ -96,22 +97,39 @@ private static edu.wpi.first.wpilibj2.command.Command driveDistanceMeters( double commandedSpeedMps = Math.copySign(clampedSpeedMps, distanceMeters); double distanceAbsMeters = Math.abs(distanceMeters); - AtomicReference startPose = new AtomicReference<>(new Pose2d()); + AtomicReference startPositions = new AtomicReference<>(); return Commands.sequence( - Commands.runOnce(() -> startPose.set(drive.getPose()), drive), + Commands.runOnce(() -> startPositions.set(drive.getPositions()), drive), Commands.runEnd( () -> drive.drive(commandedSpeedMps, 0, 0, false), () -> drive.drive(0, 0, 0, false), drive) .until( - () -> - drive.getPose().getTranslation().getDistance(startPose.get().getTranslation()) - >= distanceAbsMeters) + () -> getAverageWheelDeltaMeters(startPositions.get(), drive.getPositions()) >= distanceAbsMeters) .withTimeout(distanceAbsMeters / Math.max(0.1, Math.abs(commandedSpeedMps)) + 1.0) ); } + private static double getAverageWheelDeltaMeters( + SwerveModulePosition[] startPositions, + SwerveModulePosition[] currentPositions) { + if (startPositions == null || currentPositions == null) { + return 0.0; + } + + int count = Math.min(startPositions.length, currentPositions.length); + if (count <= 0) { + return 0.0; + } + + double sum = 0.0; + for (int i = 0; i < count; i++) { + sum += Math.abs(currentPositions[i].distanceMeters - startPositions[i].distanceMeters); + } + return sum / count; + } + private static edu.wpi.first.wpilibj2.command.Command turnRelativeDegrees( SwerveSubsystem drive, double deltaDegrees) { @@ -138,6 +156,7 @@ private static edu.wpi.first.wpilibj2.command.Command turnRelativeDegrees( return Math.abs(errorRad) < Math.toRadians(TURN_TOLERANCE_DEG); }) .withTimeout(TURN_TIMEOUT_SEC) + ); } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ce84ad8..8120df9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -129,9 +129,6 @@ public record ModuleData( public static ModuleData[] moduleData = { new ModuleData(6, 5, 7, 31.46, FRONT_LEFT, driveInvert, angleInvert), //Mod 0 Front left - // Module 1 is currently the only module oscillating; flip its angle motor invert so its - // steering closed-loop sign matches the encoder direction. - // Module 1: also invert drive so +X command drives forward like the others. new ModuleData(9, 8, 10, 49.57, FRONT_RIGHT, driveInvert, angleInvert), //Mod 1 Front right new ModuleData(12, 11, 13, 33.13, BACK_RIGHT, driveInvert, angleInvert), //Mod 2 Back right new ModuleData(15, 14, 16, 8.52, BACK_LEFT, driveInvert, angleInvert) //Mod 3 Back left From 166b09fc69ab555e907b36972a4adaa62f6abd68 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Sat, 14 Mar 2026 15:26:25 -0500 Subject: [PATCH 21/96] leftneutralzoneauto2 & rightneutralzoneauto --- .../frc/robot/Auto/LeftNeutralZoneAuto2.java | 129 ++++++++++++++++++ .../frc/robot/Auto/RightNeutralZoneAuto.java | 129 ++++++++++++++++++ src/main/java/frc/robot/Constants.java | 2 + src/main/java/frc/robot/RobotContainer.java | 4 + 4 files changed, 264 insertions(+) create mode 100644 src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java create mode 100644 src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java new file mode 100644 index 0000000..42474d6 --- /dev/null +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java @@ -0,0 +1,129 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; + + +public class LeftNeutralZoneAuto2 extends SequentialCommandGroup { + + private static final double INTAKE_POWER = -1.0; + + public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake) { + final double[] startYawRad = new double[1]; + + addCommands( + Commands.runOnce(intake::lowerIntake, intake), + + //Drive backwards 3.4m + new InstantCommand(() -> drive.drive(-3.4, 0, 0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), + + //Turn 90 degrees left + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + //Drive forward 3m + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + new InstantCommand(() -> drive.drive(3, 0, 0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 90 degrees right + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + //Drive forward 1m + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + new InstantCommand(() -> drive.drive(1, 0, 0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 90 degrees right + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + //Drive forward 3m + new InstantCommand(() -> drive.drive(3, 0, 0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), + + //Stop intake + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0,false), drive), + + // Turn 90 degrees right + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + //Drive backwards + new InstantCommand(() -> drive.drive(-1, 0, 0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 20 degrees left + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(20.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(20.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }) + + + + ); + } +} diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java new file mode 100644 index 0000000..f3f3d55 --- /dev/null +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java @@ -0,0 +1,129 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; + + +public class RightNeutralZoneAuto extends SequentialCommandGroup { + + private static final double INTAKE_POWER = -1.0; + + public RightNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake) { + final double[] startYawRad = new double[1]; + + addCommands( + Commands.runOnce(intake::lowerIntake, intake), + + //Drive backwards 3.4m + new InstantCommand(() -> drive.drive(-3.4, 0, 0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), + + //Turn 90 degrees right + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + //Drive forward 3m + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + new InstantCommand(() -> drive.drive(3, 0, 0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 90 degrees left + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + //Drive forward 1m + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + new InstantCommand(() -> drive.drive(1, 0, 0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 90 degrees left + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + //Drive forward 3m + new InstantCommand(() -> drive.drive(3, 0, 0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), + + //Stop intake + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0,false), drive), + + // Turn 90 degrees right + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }), + + //Drive forward + new InstantCommand(() -> drive.drive(1, 0, 0, false), drive), + Commands.waitSeconds(1), + new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 20 degrees right + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.run(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-20.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); + drive.drive(0,0, omegaRadiansPerSecond, false); + }, drive).until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(-20.0); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(3.0); + }) + + + + ); + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8120df9..faaab02 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -159,6 +159,7 @@ public enum AutoMode{ LeftLemonAuto, RightLemonAuto, LeftNeutralZoneAuto, + LeftNeutralZoneAuto2, RightNeutralZoneAuto, ShootEightAuto, CenterLemonAuto @@ -183,6 +184,7 @@ public static void initDashboard() { autoModeChooser.addOption("LeftLemonAuto", AutoMode.LeftLemonAuto); autoModeChooser.addOption("RightNeutralZoneAuto", AutoMode.RightNeutralZoneAuto); autoModeChooser.addOption("LeftNeutralZoneAuto", AutoMode.LeftNeutralZoneAuto); + autoModeChooser.addOption("LeftNeutralZoneAuto2", AutoMode.LeftNeutralZoneAuto2); autoModeChooser.addOption("CenterLemonAuto", AutoMode.CenterLemonAuto); SmartDashboard.putData("Auto Starting Location", sideChooser); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c8ce4b7..d6d220e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Auto.LeftLemonAuto; import frc.robot.Auto.LeftNeutralZoneAuto; +import frc.robot.Auto.LeftNeutralZoneAuto2; +import frc.robot.Auto.RightNeutralZoneAuto; import frc.robot.Auto.RightLemonAuto; import frc.robot.Auto.ShootEightAuto; import frc.robot.Auto.CenterLemonAuto; @@ -240,7 +242,9 @@ public Command getAutonomousCommand() { case DriveTestAuto -> new DriveTestAuto(m_drive, m_intake, m_shooter); case LeftLemonAuto -> new LeftLemonAuto(m_drive, m_intake, m_shooter); case RightLemonAuto -> new RightLemonAuto(m_drive, m_intake, m_shooter); + case RightNeutralZoneAuto -> new RightNeutralZoneAuto (m_drive, m_intake); case LeftNeutralZoneAuto -> new LeftNeutralZoneAuto(m_drive, m_intake, m_shooter); + case LeftNeutralZoneAuto2 -> new LeftNeutralZoneAuto2(m_drive, m_intake); case ShootEightAuto -> new ShootEightAuto(m_drive, m_intake, m_shooter); case CenterLemonAuto -> new CenterLemonAuto(m_drive, m_intake, m_shooter); From a066aa88819077d8eabce85a8b0ed20127a9cb66 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Sat, 14 Mar 2026 15:35:14 -0600 Subject: [PATCH 22/96] changes --- ...lZoneAuto.java => LeftNeutralZoneAuto1.java} | 17 +++++++++++++---- src/main/java/frc/robot/Constants.java | 14 +++++++------- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 3 files changed, 22 insertions(+), 13 deletions(-) rename src/main/java/frc/robot/Auto/{LeftNeutralZoneAuto.java => LeftNeutralZoneAuto1.java} (90%) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java similarity index 90% rename from src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java rename to src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index 23a00d9..729e939 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -17,7 +17,7 @@ import java.util.concurrent.atomic.AtomicReference; -public class LeftNeutralZoneAuto extends SequentialCommandGroup { +public class LeftNeutralZoneAuto1 extends SequentialCommandGroup { private static final double DRIVE_SPEED_MPS = 1.0; private static final double TURN_P = 4.0; private static final double TURN_TOLERANCE_DEG = 3.0; @@ -31,7 +31,7 @@ public class LeftNeutralZoneAuto extends SequentialCommandGroup { private static final double INTAKE_POWER = -1.0; - public LeftNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addRequirements(drive, intake, shooter); addCommands( @@ -56,7 +56,7 @@ public LeftNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake, Shoote // Turn 90 degrees right (intake still on). turnRelativeDegrees(drive, -90.0), - // Drive forward 3m (intake still on). + // Drive forward 3.3m (intake still on). driveDistanceMeters(drive, FORWARD_METERS_3, DRIVE_SPEED_MPS), // Stop intake at the end. @@ -64,7 +64,7 @@ public LeftNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake, Shoote Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), // Turn 90 degrees right - turnRelativeDegrees(drive, -100.0), + turnRelativeDegrees(drive, -95.0), // Drive forward (back to the trench) driveDistanceMeters(drive, -BACKWARD_METERS_2, DRIVE_SPEED_MPS), @@ -75,6 +75,15 @@ public LeftNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake, Shoote // Bring hood up to HIGH angle. Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + // Shooter + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(2.0), + // Start kicker first, then start indexer 1 second later (kicker keeps running). Commands.sequence( Commands.run(() -> { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index faaab02..675bf95 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -20,7 +20,7 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.Auto.LeftNeutralZoneAuto; +import frc.robot.Auto.LeftNeutralZoneAuto1; /** Central location for robot-wide constants grouped by subsystem and feature */ public final class Constants { @@ -128,10 +128,10 @@ public record ModuleData( ){} public static ModuleData[] moduleData = { - new ModuleData(6, 5, 7, 31.46, FRONT_LEFT, driveInvert, angleInvert), //Mod 0 Front left - new ModuleData(9, 8, 10, 49.57, FRONT_RIGHT, driveInvert, angleInvert), //Mod 1 Front right - new ModuleData(12, 11, 13, 33.13, BACK_RIGHT, driveInvert, angleInvert), //Mod 2 Back right - new ModuleData(15, 14, 16, 8.52, BACK_LEFT, driveInvert, angleInvert) //Mod 3 Back left + new ModuleData(6, 5, 7, 39.90, FRONT_LEFT, driveInvert, angleInvert), //Mod 0 Front left + new ModuleData(9, 8, 10, 46.93, FRONT_RIGHT, driveInvert, angleInvert), //Mod 1 Front right + new ModuleData(12, 11, 13, 42.09, BACK_RIGHT, driveInvert, angleInvert), //Mod 2 Back right + new ModuleData(15, 14, 16, 7.11, BACK_LEFT, driveInvert, angleInvert) //Mod 3 Back left }; } @@ -158,7 +158,7 @@ public enum AutoMode{ DriveTestAuto, LeftLemonAuto, RightLemonAuto, - LeftNeutralZoneAuto, + LeftNeutralZoneAuto1, LeftNeutralZoneAuto2, RightNeutralZoneAuto, ShootEightAuto, @@ -183,7 +183,7 @@ public static void initDashboard() { autoModeChooser.addOption("RightLemonAuto", AutoMode.RightLemonAuto); autoModeChooser.addOption("LeftLemonAuto", AutoMode.LeftLemonAuto); autoModeChooser.addOption("RightNeutralZoneAuto", AutoMode.RightNeutralZoneAuto); - autoModeChooser.addOption("LeftNeutralZoneAuto", AutoMode.LeftNeutralZoneAuto); + autoModeChooser.addOption("LeftNeutralZoneAuto1", AutoMode.LeftNeutralZoneAuto1); autoModeChooser.addOption("LeftNeutralZoneAuto2", AutoMode.LeftNeutralZoneAuto2); autoModeChooser.addOption("CenterLemonAuto", AutoMode.CenterLemonAuto); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d6d220e..43614e2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,7 +21,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Auto.LeftLemonAuto; -import frc.robot.Auto.LeftNeutralZoneAuto; +import frc.robot.Auto.LeftNeutralZoneAuto1; import frc.robot.Auto.LeftNeutralZoneAuto2; import frc.robot.Auto.RightNeutralZoneAuto; import frc.robot.Auto.RightLemonAuto; @@ -243,7 +243,7 @@ public Command getAutonomousCommand() { case LeftLemonAuto -> new LeftLemonAuto(m_drive, m_intake, m_shooter); case RightLemonAuto -> new RightLemonAuto(m_drive, m_intake, m_shooter); case RightNeutralZoneAuto -> new RightNeutralZoneAuto (m_drive, m_intake); - case LeftNeutralZoneAuto -> new LeftNeutralZoneAuto(m_drive, m_intake, m_shooter); + case LeftNeutralZoneAuto1 -> new LeftNeutralZoneAuto1(m_drive, m_intake, m_shooter); case LeftNeutralZoneAuto2 -> new LeftNeutralZoneAuto2(m_drive, m_intake); case ShootEightAuto -> new ShootEightAuto(m_drive, m_intake, m_shooter); case CenterLemonAuto -> new CenterLemonAuto(m_drive, m_intake, m_shooter); From 0936fcaa611a926f66c27e1f14f5f601cfcc60ec Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Sat, 14 Mar 2026 16:39:47 -0500 Subject: [PATCH 23/96] drive path, DriveTestAuto --- .../deploy/pathplanner/paths/New Path.path | 54 +++++++++++++++++++ .../java/frc/robot/Auto/DriveTestAuto.java | 31 +---------- 2 files changed, 56 insertions(+), 29 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/New Path.path diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..3795a64 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.9840517216435192, + "y": 7.19913353587963 + }, + "prevControl": null, + "nextControl": { + "x": 1.9840517216435192, + "y": 6.358537063666271 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.9840517216435192, + "y": 2.0811839554398146 + }, + "prevControl": { + "x": 1.9840517216435192, + "y": 2.826339672707111 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": -90.5032786928423 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -89.87322210259995 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Auto/DriveTestAuto.java b/src/main/java/frc/robot/Auto/DriveTestAuto.java index ec4cebd..0f0fa0e 100644 --- a/src/main/java/frc/robot/Auto/DriveTestAuto.java +++ b/src/main/java/frc/robot/Auto/DriveTestAuto.java @@ -16,35 +16,8 @@ public DriveTestAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsy addRequirements(drive, intake, shooter); addCommands( - drive.startAutoAt(3.562, 7.307, 0.864), - drive.autoDrive("Trench"), - drive.autoDrive("Trench2"), - Commands.deadline( - Commands.sequence( - drive.autoDrive("Neutral"), - drive.autoDrive("Neutral2"), - drive.autoDrive("Neutral3") - ), - Commands.startEnd( - () -> intake.setIntakePower(1.0), - () -> intake.setIntakePower(0.0), - intake) - ), - drive.autoDrive("Neutral4"), - Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), - Commands.sequence( - // Start kicker first, then start indexer 1 second later (kicker keeps running). - Commands.run(() -> { - shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); - shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); - shooter.setIndexerSpeed(0.0); - }, shooter).withTimeout(1.0), - Commands.run(() -> { - shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); - shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); - shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); - }, shooter) - ) + drive.startAutoAt(1.984, 7.199, -90), + drive.autoDrive("Path") ); } } From acb27dfeaabba1689815891d3774630a6b190732 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Sat, 14 Mar 2026 16:50:55 -0500 Subject: [PATCH 24/96] pathplanner changes --- src/main/deploy/pathplanner/paths/New Path.path | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 3795a64..7fe4c38 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 1.9840517216435192, - "y": 6.358537063666271 + "y": 5.727979004662265 }, "isLocked": false, "linkedName": null @@ -21,7 +21,7 @@ }, "prevControl": { "x": 1.9840517216435192, - "y": 2.826339672707111 + "y": 0.9191430558661833 }, "nextControl": null, "isLocked": false, @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -89.87322210259995 + "rotation": -89.21792778085688 }, "useDefaultConstraints": true } \ No newline at end of file From ff092e33480d6cd26dea95d72ec6cd1000f91975 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Sat, 14 Mar 2026 16:51:44 -0500 Subject: [PATCH 25/96] pathplanner changes 2 --- src/main/deploy/pathplanner/paths/New Path.path | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 7fe4c38..d83c0a8 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -90.5032786928423 + "rotation": -90.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -89.21792778085688 + "rotation": -90.0 }, "useDefaultConstraints": true } \ No newline at end of file From 4c372915bda1c1083b040b071db9f4f0b01bf326 Mon Sep 17 00:00:00 2001 From: Seqi Date: Tue, 17 Mar 2026 13:54:12 -0500 Subject: [PATCH 26/96] lowered visionCam resoution and made slightly more efficent --- src/main/java/frc/robot/Robot.java | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 055232d..d7beaef 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,6 +5,7 @@ package frc.robot; import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.UsbCamera; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -17,15 +18,22 @@ public class Robot extends TimedRobot { private final RobotContainer m_robotContainer; private final RobotSimulation m_robotSimulation; + Thread visionThread; + public Robot() { m_robotContainer = new RobotContainer(); m_robotSimulation = new RobotSimulation(m_robotContainer); + + visionThread = new Thread(() -> { + UsbCamera visionCam = CameraServer.startAutomaticCapture(); + visionCam.setResolution(640, 480); + }); + visionThread.start(); } @Override public void robotInit() { AutoConstants.initDashboard(); - CameraServer.startAutomaticCapture(); } @Override From cc188e6de08d80a4448c5c2f08942999cb593869 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Tue, 17 Mar 2026 16:45:47 -0500 Subject: [PATCH 27/96] auto adjustments --- .../frc/robot/Auto/LeftNeutralZoneAuto1.java | 10 ++--- .../frc/robot/Auto/LeftNeutralZoneAuto2.java | 43 +++++++++++++++---- src/main/java/frc/robot/RobotContainer.java | 2 +- 3 files changed, 40 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index 729e939..a5745d1 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -23,10 +23,10 @@ public class LeftNeutralZoneAuto1 extends SequentialCommandGroup { private static final double TURN_TOLERANCE_DEG = 3.0; private static final double TURN_TIMEOUT_SEC = 2.5; - private static final double BACKWARD_METERS_1 = 3.4; + private static final double BACKWARD_METERS_1 = 3.6; private static final double BACKWARD_METERS_2 = 3.0; private static final double FORWARD_METERS_1 = 3.0; - private static final double FORWARD_METERS_2 = 1.0; + private static final double FORWARD_METERS_2 = 1.; private static final double FORWARD_METERS_3 = 3.3; private static final double INTAKE_POWER = -1.0; @@ -37,7 +37,7 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot addCommands( Commands.runOnce(intake::lowerIntake, intake), - // Drive backwards 3.4m. + // Drive backwards 3.6m. driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS), // Turn 90 degrees left. @@ -50,7 +50,7 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot // Turn 90 degrees right (intake still on). turnRelativeDegrees(drive, -90.0), - // Drive forward 1m (intake still on). + // Drive forward 1.3m (intake still on). driveDistanceMeters(drive, FORWARD_METERS_2, DRIVE_SPEED_MPS), // Turn 90 degrees right (intake still on). @@ -66,7 +66,7 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot // Turn 90 degrees right turnRelativeDegrees(drive, -95.0), - // Drive forward (back to the trench) + // Drive backward (back to the trench) driveDistanceMeters(drive, -BACKWARD_METERS_2, DRIVE_SPEED_MPS), // Turn 10 degrees left diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java index 42474d6..29c8ad4 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java @@ -20,14 +20,14 @@ public class LeftNeutralZoneAuto2 extends SequentialCommandGroup { private static final double INTAKE_POWER = -1.0; - public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake) { + public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { final double[] startYawRad = new double[1]; addCommands( Commands.runOnce(intake::lowerIntake, intake), - //Drive backwards 3.4m - new InstantCommand(() -> drive.drive(-3.4, 0, 0, false), drive), + //Drive backwards 3.6m + new InstantCommand(() -> drive.drive(-3.6, 0, 0, false), drive), Commands.waitSeconds(1), new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), @@ -63,9 +63,9 @@ public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake) { return Math.abs(errorRad) < Math.toRadians(3.0); }), - //Drive forward 1m + //Drive forward 1.3m Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), - new InstantCommand(() -> drive.drive(1, 0, 0, false), drive), + new InstantCommand(() -> drive.drive(1.3, 0, 0, false), drive), Commands.waitSeconds(1), new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), @@ -109,19 +109,44 @@ public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake) { Commands.waitSeconds(1), new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), - // Turn 20 degrees left + // Turn 10 degrees left Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), Commands.run(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(20.0); + double targetYawRad = startYawRad[0] + Math.toRadians(10.0); double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); drive.drive(0,0, omegaRadiansPerSecond, false); }, drive).until(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(20.0); + double targetYawRad = startYawRad[0] + Math.toRadians(10.0); double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); return Math.abs(errorRad) < Math.toRadians(3.0); - }) + }), + + //Bring hood up to HIGH angle + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + + //Shooter + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(2.0), + Commands.sequence( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ) ); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 43614e2..2a98de6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -244,7 +244,7 @@ public Command getAutonomousCommand() { case RightLemonAuto -> new RightLemonAuto(m_drive, m_intake, m_shooter); case RightNeutralZoneAuto -> new RightNeutralZoneAuto (m_drive, m_intake); case LeftNeutralZoneAuto1 -> new LeftNeutralZoneAuto1(m_drive, m_intake, m_shooter); - case LeftNeutralZoneAuto2 -> new LeftNeutralZoneAuto2(m_drive, m_intake); + case LeftNeutralZoneAuto2 -> new LeftNeutralZoneAuto2(m_drive, m_intake, m_shooter); case ShootEightAuto -> new ShootEightAuto(m_drive, m_intake, m_shooter); case CenterLemonAuto -> new CenterLemonAuto(m_drive, m_intake, m_shooter); From 9ae7df584f71c35c9102573ddaf9a5350fec8d81 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Tue, 17 Mar 2026 19:03:17 -0500 Subject: [PATCH 28/96] fix drift problem (maybe) --- .../frc/robot/Auto/LeftNeutralZoneAuto1.java | 21 +++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index a5745d1..d3b249b 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -19,6 +19,8 @@ public class LeftNeutralZoneAuto1 extends SequentialCommandGroup { private static final double DRIVE_SPEED_MPS = 1.0; + private static final double DRIVE_HEADING_P = 3.0; + private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; private static final double TURN_TOLERANCE_DEG = 3.0; private static final double TURN_TIMEOUT_SEC = 2.5; @@ -27,7 +29,7 @@ public class LeftNeutralZoneAuto1 extends SequentialCommandGroup { private static final double BACKWARD_METERS_2 = 3.0; private static final double FORWARD_METERS_1 = 3.0; private static final double FORWARD_METERS_2 = 1.; - private static final double FORWARD_METERS_3 = 3.3; + private static final double FORWARD_METERS_3 = 3.4; private static final double INTAKE_POWER = -1.0; @@ -56,7 +58,7 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot // Turn 90 degrees right (intake still on). turnRelativeDegrees(drive, -90.0), - // Drive forward 3.3m (intake still on). + // Drive forward 3.4m (intake still on). driveDistanceMeters(drive, FORWARD_METERS_3, DRIVE_SPEED_MPS), // Stop intake at the end. @@ -107,11 +109,22 @@ private static edu.wpi.first.wpilibj2.command.Command driveDistanceMeters( double distanceAbsMeters = Math.abs(distanceMeters); AtomicReference startPositions = new AtomicReference<>(); + final double[] startYawRad = new double[1]; return Commands.sequence( - Commands.runOnce(() -> startPositions.set(drive.getPositions()), drive), + Commands.runOnce(() -> { + startPositions.set(drive.getPositions()); + startYawRad[0] = drive.getYaw().getRadians(); + }, drive), Commands.runEnd( - () -> drive.drive(commandedSpeedMps, 0, 0, false), + () -> { + double errorRad = MathUtil.angleModulus(startYawRad[0] - drive.getYaw().getRadians()); + double maxOmegaRadPerSec = + Math.min(DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC, SwerveConstants.maxAngularVelocity); + double omegaRadPerSec = + MathUtil.clamp(errorRad * DRIVE_HEADING_P, -maxOmegaRadPerSec, maxOmegaRadPerSec); + drive.drive(commandedSpeedMps, 0, omegaRadPerSec, false); + }, () -> drive.drive(0, 0, 0, false), drive) .until( From bc0ffffaf56fb901f107a3db4af1a677cc801ccd Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Tue, 17 Mar 2026 19:33:58 -0500 Subject: [PATCH 29/96] auto position adjustments --- src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java | 8 ++++---- src/main/java/frc/robot/Constants.java | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index d3b249b..c03842d 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -18,7 +18,7 @@ public class LeftNeutralZoneAuto1 extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 1.0; + private static final double DRIVE_SPEED_MPS = 2; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; @@ -29,7 +29,7 @@ public class LeftNeutralZoneAuto1 extends SequentialCommandGroup { private static final double BACKWARD_METERS_2 = 3.0; private static final double FORWARD_METERS_1 = 3.0; private static final double FORWARD_METERS_2 = 1.; - private static final double FORWARD_METERS_3 = 3.4; + private static final double FORWARD_METERS_3 = 3.2; private static final double INTAKE_POWER = -1.0; @@ -71,8 +71,8 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot // Drive backward (back to the trench) driveDistanceMeters(drive, -BACKWARD_METERS_2, DRIVE_SPEED_MPS), - // Turn 10 degrees left - turnRelativeDegrees(drive, 10.0), + // Turn 20 degrees left + turnRelativeDegrees(drive, 20.0), // Bring hood up to HIGH angle. Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 675bf95..637802f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -298,7 +298,7 @@ public static final class ShooterConstants { // Max travel is 3 rotations = 1080 degrees. public static final double HOOD_MIN_ROTATIONS = 0.0; public static final double HOOD_MED_ROTATIONS = 20.0; - public static final double HOOD_MAX_ROTATIONS = 23.0; + public static final double HOOD_MAX_ROTATIONS = 27.0; // Preset positions. public static final double HOOD_ANGLE_LOW = HOOD_MIN_ROTATIONS; From c568b5f095834bd5ccc76a08238254737e668055 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 18 Mar 2026 16:42:18 -0500 Subject: [PATCH 30/96] Auto Add intake arm movement to LeftNeutralZoneAuto. Adjust CenterToDepot and RightNeutralZoneAuto, need testing. --- .../frc/robot/Auto/CenterToDepotAuto.java | 173 ++++++++++++++++ .../java/frc/robot/Auto/EightLemonAuto.java | 20 -- .../frc/robot/Auto/LeftNeutralZoneAuto1.java | 42 ++-- .../frc/robot/Auto/RightNeutralZoneAuto.java | 129 ------------ .../frc/robot/Auto/RightNeutralZoneAuto1.java | 184 ++++++++++++++++++ src/main/java/frc/robot/Constants.java | 10 +- src/main/java/frc/robot/RobotContainer.java | 9 +- .../robot/UnusedAuto/CenterToDepotAuto.java | 91 --------- .../frc/robot/UnusedAuto/DriveTestAuto.java | 32 --- .../LeftNeutralZoneAuto2.java | 2 +- 10 files changed, 397 insertions(+), 295 deletions(-) create mode 100644 src/main/java/frc/robot/Auto/CenterToDepotAuto.java delete mode 100644 src/main/java/frc/robot/Auto/EightLemonAuto.java delete mode 100644 src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java create mode 100644 src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java delete mode 100644 src/main/java/frc/robot/UnusedAuto/CenterToDepotAuto.java delete mode 100644 src/main/java/frc/robot/UnusedAuto/DriveTestAuto.java rename src/main/java/frc/robot/{Auto => UnusedAuto}/LeftNeutralZoneAuto2.java (99%) diff --git a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java new file mode 100644 index 0000000..88d5396 --- /dev/null +++ b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java @@ -0,0 +1,173 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import java.util.concurrent.atomic.AtomicReference; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; +import frc.robot.Subsystems.SwerveSubsystem; + +public class CenterToDepotAuto extends SequentialCommandGroup { + private static final double DRIVE_SPEED_MPS = 2; + private static final double DRIVE_HEADING_P = 3.0; + private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; + private static final double TURN_P = 4.0; + private static final double TURN_TOLERANCE_DEG = 3.0; + private static final double TURN_TIMEOUT_SEC = 2.5; + + private static final double BACKWARD_METERS_1 = 1; + private static final double FORWARD_METERS_1 = 2; + + private static final double INTAKE_POWER = -1.0; + + public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + addRequirements(drive, intake, shooter); + + addCommands( + Commands.runOnce(intake::lowerIntake, intake), + + // Drive forwards 2m. + driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Turn 90 degrees right. + turnRelativeDegrees(drive, -90.0), + + // Drive forward 2m + driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Turn 90 degrees left (intake on). + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + turnRelativeDegrees(drive, 90.0), + + // Drive backwards 1m (intake still on). + driveDistanceMeters(drive, BACKWARD_METERS_1, DRIVE_SPEED_MPS), + + // Stop intake at the end. + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 90 degrees right (intake still on). + turnRelativeDegrees(drive, -90.0), + + // Drive backward 1m (intake still on). + driveDistanceMeters(drive, BACKWARD_METERS_1, DRIVE_SPEED_MPS), + + + // Bring hood up to HIGH angle. + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + + // Shooter + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(2.0), + + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ) + ); + } + + private static edu.wpi.first.wpilibj2.command.Command driveDistanceMeters( + SwerveSubsystem drive, + double distanceMeters, + double speedMps) { + double clampedSpeedMps = MathUtil.clamp(Math.abs(speedMps), 0.0, SwerveConstants.maxSpeed); + double commandedSpeedMps = Math.copySign(clampedSpeedMps, distanceMeters); + double distanceAbsMeters = Math.abs(distanceMeters); + + AtomicReference startPositions = new AtomicReference<>(); + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> { + startPositions.set(drive.getPositions()); + startYawRad[0] = drive.getYaw().getRadians(); + }, drive), + Commands.runEnd( + () -> { + double errorRad = MathUtil.angleModulus(startYawRad[0] - drive.getYaw().getRadians()); + double maxOmegaRadPerSec = + Math.min(DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC, SwerveConstants.maxAngularVelocity); + double omegaRadPerSec = + MathUtil.clamp(errorRad * DRIVE_HEADING_P, -maxOmegaRadPerSec, maxOmegaRadPerSec); + drive.drive(commandedSpeedMps, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until( + () -> getAverageWheelDeltaMeters(startPositions.get(), drive.getPositions()) >= distanceAbsMeters) + .withTimeout(distanceAbsMeters / Math.max(0.1, Math.abs(commandedSpeedMps)) + 1.0) + ); + } + + private static double getAverageWheelDeltaMeters( + SwerveModulePosition[] startPositions, + SwerveModulePosition[] currentPositions) { + if (startPositions == null || currentPositions == null) { + return 0.0; + } + + int count = Math.min(startPositions.length, currentPositions.length); + if (count <= 0) { + return 0.0; + } + + double sum = 0.0; + for (int i = 0; i < count; i++) { + sum += Math.abs(currentPositions[i].distanceMeters - startPositions[i].distanceMeters); + } + return sum / count; + } + + private static edu.wpi.first.wpilibj2.command.Command turnRelativeDegrees( + SwerveSubsystem drive, + double deltaDegrees) { + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.runEnd( + () -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadPerSec = + MathUtil.clamp( + errorRad * TURN_P, + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + drive.drive(0, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(TURN_TOLERANCE_DEG); + }) + .withTimeout(TURN_TIMEOUT_SEC) + + ); + } +} diff --git a/src/main/java/frc/robot/Auto/EightLemonAuto.java b/src/main/java/frc/robot/Auto/EightLemonAuto.java deleted file mode 100644 index 5f99ce3..0000000 --- a/src/main/java/frc/robot/Auto/EightLemonAuto.java +++ /dev/null @@ -1,20 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.Auto; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Subsystems.ShooterSubsystem; -import frc.robot.Subsystems.IntakeSubsystem; -import frc.robot.Subsystems.SwerveSubsystem; - - -public class EightLemonAuto extends SequentialCommandGroup { - public EightLemonAuto (SwerveSubsystem drive, ShooterSubsystem shooter, IntakeSubsystem intake){ - addCommands( - drive.startAutoAt(3.53, 7.13, -130.45), - drive.autoDrive("8FuelPath") - ); - } -} diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index c03842d..efa69a8 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.Subsystems.SwerveSubsystem; @@ -66,7 +67,7 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), // Turn 90 degrees right - turnRelativeDegrees(drive, -95.0), + turnRelativeDegrees(drive, -90.0), // Drive backward (back to the trench) driveDistanceMeters(drive, -BACKWARD_METERS_2, DRIVE_SPEED_MPS), @@ -84,18 +85,33 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot }, shooter), Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) - .withTimeout(2.0), - - // Start kicker first, then start indexer 1 second later (kicker keeps running). - Commands.sequence( - Commands.run(() -> { - shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); - shooter.setIndexerSpeed(0.0); - }, shooter).withTimeout(1.0), - Commands.run(() -> { - shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); - shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); - }, shooter) + .withTimeout(1.0), + + Commands.parallel( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ), + + // While shooting/indexing, continuously move the intake arm up/down. + Commands.sequence( + Commands.runOnce(intake::lowerIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), + Commands.runOnce(intake::raiseIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) + ) + .repeatedly() ) ); } diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java deleted file mode 100644 index f3f3d55..0000000 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto.java +++ /dev/null @@ -1,129 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.Auto; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants.ShooterConstants; -import frc.robot.Constants.SwerveConstants; -import frc.robot.Subsystems.SwerveSubsystem; -import frc.robot.Subsystems.IntakeSubsystem; -import frc.robot.Subsystems.ShooterSubsystem; - - -public class RightNeutralZoneAuto extends SequentialCommandGroup { - - private static final double INTAKE_POWER = -1.0; - - public RightNeutralZoneAuto(SwerveSubsystem drive, IntakeSubsystem intake) { - final double[] startYawRad = new double[1]; - - addCommands( - Commands.runOnce(intake::lowerIntake, intake), - - //Drive backwards 3.4m - new InstantCommand(() -> drive.drive(-3.4, 0, 0, false), drive), - Commands.waitSeconds(1), - new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), - - //Turn 90 degrees right - Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), - Commands.run(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); - drive.drive(0,0, omegaRadiansPerSecond, false); - }, drive).until(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - return Math.abs(errorRad) < Math.toRadians(3.0); - }), - - //Drive forward 3m - Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), - new InstantCommand(() -> drive.drive(3, 0, 0, false), drive), - Commands.waitSeconds(1), - new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), - - // Turn 90 degrees left - Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), - Commands.run(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(90.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); - drive.drive(0,0, omegaRadiansPerSecond, false); - }, drive).until(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(90.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - return Math.abs(errorRad) < Math.toRadians(3.0); - }), - - //Drive forward 1m - Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), - new InstantCommand(() -> drive.drive(1, 0, 0, false), drive), - Commands.waitSeconds(1), - new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), - - // Turn 90 degrees left - Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), - Commands.run(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(90.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); - drive.drive(0,0, omegaRadiansPerSecond, false); - }, drive).until(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(90.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - return Math.abs(errorRad) < Math.toRadians(3.0); - }), - - //Drive forward 3m - new InstantCommand(() -> drive.drive(3, 0, 0, false), drive), - Commands.waitSeconds(1), - new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), - - //Stop intake - Commands.runOnce(() -> intake.setIntakePower(0.0), intake), - Commands.runOnce(() -> drive.drive(0, 0, 0,false), drive), - - // Turn 90 degrees right - Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), - Commands.run(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); - drive.drive(0,0, omegaRadiansPerSecond, false); - }, drive).until(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(-90.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - return Math.abs(errorRad) < Math.toRadians(3.0); - }), - - //Drive forward - new InstantCommand(() -> drive.drive(1, 0, 0, false), drive), - Commands.waitSeconds(1), - new InstantCommand(() -> drive.drive(0, 0, 0, false), drive), - - // Turn 20 degrees right - Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), - Commands.run(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(-20.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); - drive.drive(0,0, omegaRadiansPerSecond, false); - }, drive).until(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(-20.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - return Math.abs(errorRad) < Math.toRadians(3.0); - }) - - - - ); - } -} diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java new file mode 100644 index 0000000..0d79712 --- /dev/null +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java @@ -0,0 +1,184 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; + +import java.util.concurrent.atomic.AtomicReference; + + +public class RightNeutralZoneAuto1 extends SequentialCommandGroup { + private static final double DRIVE_SPEED_MPS = 2; + private static final double DRIVE_HEADING_P = 3.0; + private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; + private static final double TURN_P = 4.0; + private static final double TURN_TOLERANCE_DEG = 3.0; + private static final double TURN_TIMEOUT_SEC = 2.5; + + private static final double BACKWARD_METERS_1 = 3.6; + private static final double BACKWARD_METERS_2 = 3.0; + private static final double FORWARD_METERS_1 = 3.0; + private static final double FORWARD_METERS_2 = 1.; + private static final double FORWARD_METERS_3 = 3.2; + + private static final double INTAKE_POWER = -1.0; + + public RightNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + addRequirements(drive, intake, shooter); + + addCommands( + Commands.runOnce(intake::lowerIntake, intake), + + // Drive backwards 3.6m. + driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS), + + // Turn 90 degrees right. + turnRelativeDegrees(drive, -90.0), + + // Drive forward 3m while starting intake (intake stays on for the rest of auto). + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Turn 90 degrees left (intake still on). + turnRelativeDegrees(drive, 90.0), + + // Drive forward 1.3m (intake still on). + driveDistanceMeters(drive, FORWARD_METERS_2, DRIVE_SPEED_MPS), + + // Turn 90 degrees left (intake still on). + turnRelativeDegrees(drive, 90.0), + + // Drive forward 3.4m (intake still on). + driveDistanceMeters(drive, FORWARD_METERS_3, DRIVE_SPEED_MPS), + + // Stop intake at the end. + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 90 degrees left + turnRelativeDegrees(drive, 90.0), + + // Drive backward (back to the trench) + driveDistanceMeters(drive, -BACKWARD_METERS_2, DRIVE_SPEED_MPS), + + // Turn 20 degrees right + turnRelativeDegrees(drive, -20.0), + + // Bring hood up to HIGH angle. + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + + // Shooter + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(2.0), + + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ) + ); + } + + private static edu.wpi.first.wpilibj2.command.Command driveDistanceMeters( + SwerveSubsystem drive, + double distanceMeters, + double speedMps) { + double clampedSpeedMps = MathUtil.clamp(Math.abs(speedMps), 0.0, SwerveConstants.maxSpeed); + double commandedSpeedMps = Math.copySign(clampedSpeedMps, distanceMeters); + double distanceAbsMeters = Math.abs(distanceMeters); + + AtomicReference startPositions = new AtomicReference<>(); + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> { + startPositions.set(drive.getPositions()); + startYawRad[0] = drive.getYaw().getRadians(); + }, drive), + Commands.runEnd( + () -> { + double errorRad = MathUtil.angleModulus(startYawRad[0] - drive.getYaw().getRadians()); + double maxOmegaRadPerSec = + Math.min(DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC, SwerveConstants.maxAngularVelocity); + double omegaRadPerSec = + MathUtil.clamp(errorRad * DRIVE_HEADING_P, -maxOmegaRadPerSec, maxOmegaRadPerSec); + drive.drive(commandedSpeedMps, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until( + () -> getAverageWheelDeltaMeters(startPositions.get(), drive.getPositions()) >= distanceAbsMeters) + .withTimeout(distanceAbsMeters / Math.max(0.1, Math.abs(commandedSpeedMps)) + 1.0) + ); + } + + private static double getAverageWheelDeltaMeters( + SwerveModulePosition[] startPositions, + SwerveModulePosition[] currentPositions) { + if (startPositions == null || currentPositions == null) { + return 0.0; + } + + int count = Math.min(startPositions.length, currentPositions.length); + if (count <= 0) { + return 0.0; + } + + double sum = 0.0; + for (int i = 0; i < count; i++) { + sum += Math.abs(currentPositions[i].distanceMeters - startPositions[i].distanceMeters); + } + return sum / count; + } + + private static edu.wpi.first.wpilibj2.command.Command turnRelativeDegrees( + SwerveSubsystem drive, + double deltaDegrees) { + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.runEnd( + () -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadPerSec = + MathUtil.clamp( + errorRad * TURN_P, + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + drive.drive(0, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(TURN_TOLERANCE_DEG); + }) + .withTimeout(TURN_TIMEOUT_SEC) + + ); + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 637802f..f4672f4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -159,10 +159,10 @@ public enum AutoMode{ LeftLemonAuto, RightLemonAuto, LeftNeutralZoneAuto1, - LeftNeutralZoneAuto2, - RightNeutralZoneAuto, + RightNeutralZoneAuto1, ShootEightAuto, - CenterLemonAuto + CenterLemonAuto, + CenterToDepotAuto } private static SendableChooser sideChooser = new SendableChooser(); @@ -182,10 +182,10 @@ public static void initDashboard() { autoModeChooser.addOption("ShootEightAuto", AutoMode.ShootEightAuto); autoModeChooser.addOption("RightLemonAuto", AutoMode.RightLemonAuto); autoModeChooser.addOption("LeftLemonAuto", AutoMode.LeftLemonAuto); - autoModeChooser.addOption("RightNeutralZoneAuto", AutoMode.RightNeutralZoneAuto); + autoModeChooser.addOption("RightNeutralZoneAuto1", AutoMode.RightNeutralZoneAuto1); autoModeChooser.addOption("LeftNeutralZoneAuto1", AutoMode.LeftNeutralZoneAuto1); - autoModeChooser.addOption("LeftNeutralZoneAuto2", AutoMode.LeftNeutralZoneAuto2); autoModeChooser.addOption("CenterLemonAuto", AutoMode.CenterLemonAuto); + autoModeChooser.addOption("CenterToDepotAuto", AutoMode.CenterToDepotAuto); SmartDashboard.putData("Auto Starting Location", sideChooser); SmartDashboard.putData("Auto Mode", autoModeChooser); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2a98de6..f0d2dfa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,11 +22,11 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Auto.LeftLemonAuto; import frc.robot.Auto.LeftNeutralZoneAuto1; -import frc.robot.Auto.LeftNeutralZoneAuto2; -import frc.robot.Auto.RightNeutralZoneAuto; +import frc.robot.Auto.RightNeutralZoneAuto1; import frc.robot.Auto.RightLemonAuto; import frc.robot.Auto.ShootEightAuto; import frc.robot.Auto.CenterLemonAuto; +import frc.robot.Auto.CenterToDepotAuto; import frc.robot.Auto.DriveTestAuto; import frc.robot.Constants.AutoConstants; import frc.robot.Constants.VisionConstants; @@ -37,6 +37,7 @@ import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.ShooterSubsystem; import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.UnusedAuto.LeftNeutralZoneAuto2; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.DriverStation; @@ -242,11 +243,11 @@ public Command getAutonomousCommand() { case DriveTestAuto -> new DriveTestAuto(m_drive, m_intake, m_shooter); case LeftLemonAuto -> new LeftLemonAuto(m_drive, m_intake, m_shooter); case RightLemonAuto -> new RightLemonAuto(m_drive, m_intake, m_shooter); - case RightNeutralZoneAuto -> new RightNeutralZoneAuto (m_drive, m_intake); + case RightNeutralZoneAuto1 -> new RightNeutralZoneAuto1 (m_drive, m_intake, m_shooter); case LeftNeutralZoneAuto1 -> new LeftNeutralZoneAuto1(m_drive, m_intake, m_shooter); - case LeftNeutralZoneAuto2 -> new LeftNeutralZoneAuto2(m_drive, m_intake, m_shooter); case ShootEightAuto -> new ShootEightAuto(m_drive, m_intake, m_shooter); case CenterLemonAuto -> new CenterLemonAuto(m_drive, m_intake, m_shooter); + case CenterToDepotAuto -> new CenterToDepotAuto(m_drive, m_intake, m_shooter); default -> Commands.none(); diff --git a/src/main/java/frc/robot/UnusedAuto/CenterToDepotAuto.java b/src/main/java/frc/robot/UnusedAuto/CenterToDepotAuto.java deleted file mode 100644 index 34a761f..0000000 --- a/src/main/java/frc/robot/UnusedAuto/CenterToDepotAuto.java +++ /dev/null @@ -1,91 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.UnusedAuto; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants.SwerveConstants; -import frc.robot.Subsystems.SwerveSubsystem; - -public class CenterToDepotAuto extends SequentialCommandGroup { - public CenterToDepotAuto (SwerveSubsystem drive) { - final double[] startYawRad = new double[1]; - addCommands( - drive.startAutoAt(4.61, 4.03, 90.0), - new InstantCommand(()->drive.drive(0,0.5,0, false), drive), - Commands.waitSeconds(2), - new InstantCommand(()->drive.drive(0,0,0, false),drive), - - Commands.waitSeconds(1), - - //SHOOT - - new InstantCommand(()-> drive.drive(0.9,0,0,false),drive), - Commands.waitSeconds(2), - new InstantCommand(()->drive.drive(0,0,0,false), drive), - - //Turn ~90 degrees in place (robot-centric) - Commands.runOnce(()->startYawRad[0] = drive.getYaw().getRadians(), drive), - Commands.run(()->{ - double targetYawRad = startYawRad[0] + (Math.PI / 2.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); - drive.drive(0,0,omegaRadiansPerSecond, false); - }, drive).until(()->{ - double targetYawRad = startYawRad[0] + (Math.PI / 2.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - return Math.abs(errorRad) < Math.toRadians(3.0); - }), - new InstantCommand(() -> drive.drive(0,0,0, false), drive), - - // Move forward ~1m (0.5 m/s for 2s) after turning (to the depot) - new InstantCommand(() -> drive.drive(0.7,0,0, false), drive), - Commands.waitSeconds(2), - new InstantCommand(() -> drive.drive(0,0,0, false), drive), - - Commands.waitSeconds(2), - - //INTAKE - - - // Back up ~0.5m, then turn 180 degrees - new InstantCommand(() -> drive.drive(-0.5,0,0, false), drive), - Commands.waitSeconds(1), - new InstantCommand(() -> drive.drive(0,0,0, false), drive), - Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), - Commands.run(()->{ - double targetYawRad = startYawRad[0] + Math.PI; - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); - drive.drive(0,0,omegaRadiansPerSecond, false); - }, drive).until(()->{ - double targetYawRad = startYawRad[0] + Math.PI; - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - return Math.abs(errorRad) < Math.toRadians(3.0); - }), - - new InstantCommand(() -> drive.drive(0.4, 0, 0, false), drive), - Commands.waitSeconds(2), - new InstantCommand(() -> drive.drive(0,0,0, false), drive), - - //Turn 40 degrees left (counterclockwise) - Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), - Commands.run(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(40.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - double omegaRadiansPerSecond = MathUtil.clamp(errorRad * 4.0, -SwerveConstants.maxAngularVelocity, SwerveConstants.maxAngularVelocity); - drive.drive(0,0, omegaRadiansPerSecond, false); - }, drive).until(() -> { - double targetYawRad = startYawRad[0] + Math.toRadians(40.0); - double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); - return Math.abs(errorRad) < Math.toRadians(3.0); - }), - new InstantCommand(() -> drive.drive(0,0,0, false), drive) - ); - } -} - diff --git a/src/main/java/frc/robot/UnusedAuto/DriveTestAuto.java b/src/main/java/frc/robot/UnusedAuto/DriveTestAuto.java deleted file mode 100644 index 105578a..0000000 --- a/src/main/java/frc/robot/UnusedAuto/DriveTestAuto.java +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.UnusedAuto; - - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Subsystems.SwerveSubsystem; - -/* -public class DriveTestAuto extends SequentialCommandGroup { - public DriveTestAuto (SwerveSubsystem drive) { - addCommands( - new InstantCommand(() -> drive.drive(0.5,0,0, false), drive), - Commands.waitSeconds(2), - new InstantCommand(() -> drive.drive(0,0,0, false), drive) - ); - } -} -*/ - - - -public class DriveTestAuto extends SequentialCommandGroup { - public DriveTestAuto (SwerveSubsystem drive){ - addCommands( - drive.startAutoAt(1.165, 6.000, 0.000), - drive.autoDrive("DriveTestPath") - ); - } -} diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java b/src/main/java/frc/robot/UnusedAuto/LeftNeutralZoneAuto2.java similarity index 99% rename from src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java rename to src/main/java/frc/robot/UnusedAuto/LeftNeutralZoneAuto2.java index 29c8ad4..fed70d8 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java +++ b/src/main/java/frc/robot/UnusedAuto/LeftNeutralZoneAuto2.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.Auto; +package frc.robot.UnusedAuto; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.kinematics.SwerveModulePosition; From c1c6dfb406c8c2720dd9dac4325c0df84a5bfc1b Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Thu, 19 Mar 2026 20:41:00 -0500 Subject: [PATCH 31/96] Tuned the driving speed up to 85% --- src/main/java/frc/robot/RobotContainer.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f0d2dfa..cca442d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,7 +37,6 @@ import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.ShooterSubsystem; import frc.robot.Subsystems.SwerveSubsystem; -import frc.robot.UnusedAuto.LeftNeutralZoneAuto2; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.DriverStation; @@ -200,9 +199,9 @@ private void configureBindings() { // SwerveSubsystem - The drive subsystem to control m_drive, // translationSupplier - Forward/backward speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.7, + () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.85, // strafeSupplier - Side-to-side speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.7, + () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.85, // rotationSupplier - Rotation speed () -> -driveController.getRawAxis(rotationAxis) * 0.5, // robotCentricSupplier - Robot-oriented (true) vs field-oriented (false) From 47018b28f69ae919210d15e83743529c56b87431 Mon Sep 17 00:00:00 2001 From: Gavin Date: Fri, 20 Mar 2026 21:58:54 -0500 Subject: [PATCH 32/96] Pre10k odometry update (#28) * fixed spelling of variable, added some safeguards for limelights, change back to megatag 2, lowered standard deviation for translation, updated updateOdometryWithVision method * Added tuningHelper class * changed tuning variables to only work in test mode * updated spelling mistake in gitignore for ctre_sim folder * added second intake arm motor (will probably need to be fixed) * added feedforward for intake arm * fixed feedforward (whoops but is okay now) * Add the id for the 2nd intake arm motor * Intake arm 2 * intake arm 2 tuning --------- Co-authored-by: Nonochen0104 Co-authored-by: Lilly <113945482+raininglilly@users.noreply.github.com> --- .gitignore | 2 +- src/main/java/frc/robot/Constants.java | 22 ++++--- src/main/java/frc/robot/Robot.java | 4 ++ .../frc/robot/Subsystems/IntakeSubsystem.java | 54 +++++++++++++++-- .../frc/robot/Subsystems/SwerveSubsystem.java | 42 ++++++++----- .../java/frc/robot/Tuning/TuningHelper.java | 59 +++++++++++++++++++ 6 files changed, 157 insertions(+), 26 deletions(-) create mode 100644 src/main/java/frc/robot/Tuning/TuningHelper.java diff --git a/.gitignore b/.gitignore index 2c7dcf3..c5c9340 100644 --- a/.gitignore +++ b/.gitignore @@ -55,4 +55,4 @@ Thumbs.db .vscode/ # sim files -.ctre_sim/ +ctre_sim/ diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f4672f4..58712f5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -150,7 +150,7 @@ public static final class AutoConstants { public static final RobotConfig ROBOT_CONFIG = new RobotConfig (52, 6.8, MODULE_CONFIG, SwerveConstants.FRONT_LEFT, SwerveConstants.FRONT_RIGHT, SwerveConstants.BACK_LEFT, SwerveConstants.BACK_RIGHT); - public static final PPHolonomicDriveController SWERV_DRIVE_CONTROLLER = new PPHolonomicDriveController(new PIDConstants(5.0,0.00001,0.0), + public static final PPHolonomicDriveController SWERVE_DRIVE_CONTROLLER = new PPHolonomicDriveController(new PIDConstants(5.0,0.00001,0.0), new PIDConstants(5.0, 0.005, 0.001) ); public enum AutoMode{ @@ -257,8 +257,9 @@ public static final class VisionConstants { public static final double MAX_VISION_ANGULAR_RATE_DEG_PER_SEC = 720.0; /** Standard deviations for vision measurements: (x meters, y meters, theta radians). */ - public static final double VISION_STD_DEV_X_METERS = 0.7; - public static final double VISION_STD_DEV_Y_METERS = 0.7; + //Rotation (Radians) should be handled by pigion so deviation is high + public static final double VISION_STD_DEV_X_METERS = 0.4; + public static final double VISION_STD_DEV_Y_METERS = 0.4; public static final double VISION_STD_DEV_THETA_RADIANS = 99999.0; public static String getLimelightStreamUrl(String limelightName) { @@ -317,10 +318,11 @@ public static final class IntakeConstants { public static double INTAKE_SPEED = 0.90; // max percent output for intake motor public static int INTAKE_ARM_ID = 18; + public static int INTAKE_ARM_2_ID = 24; public static int GEAR_RATIO = 25; //Intake arm position units are degrees - public static final double INTAKE_ARM_MIN_DEG = 20.0; + public static final double INTAKE_ARM_MIN_DEG = 25.0; public static final double INTAKE_ARM_MAX_DEG = 90.0; //Preset positions @@ -328,11 +330,17 @@ public static final class IntakeConstants { public static final double INTAKE_ARM_RAISED_POSITION = INTAKE_ARM_MAX_DEG; //PID constants for intake arm (degrees). - public static final double INTAKE_ARM_kP = 6.0; - public static final double INTAKE_ARM_kI = 1.5; - public static final double INTAKE_ARM_kD = 0.15; + public static final double INTAKE_ARM_kP = 9.90; + public static final double INTAKE_ARM_kI = 1.60; + public static final double INTAKE_ARM_kD = 0.70; public static final double INTAKE_ARM_TOLERANCE_DEG = 2.0; + //Feedforward constants for intake arm + public static final double INTAKE_ARM_kS = 0.0; + public static final double INTAKE_ARM_kG = 0.0; + public static final double INTAKE_ARM_kV = 0.0; + public static final double INTAKE_ARM_kA = 0.0; + //Percent output cap (0..1) for gentler motion //duty-cycle / percent output for SparkMax.set(...), which expects a value in [-1.0, 1.0] public static final double INTAKE_ARM_MAX_OUTPUT = 0.20; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d7beaef..8166f38 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,11 +10,14 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Constants.AutoConstants; +import frc.robot.Tuning.TuningHelper; public class Robot extends TimedRobot { private Command m_autonomousCommand; + private TuningHelper tuning = new TuningHelper(); + private final RobotContainer m_robotContainer; private final RobotSimulation m_robotSimulation; @@ -39,6 +42,7 @@ public void robotInit() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + tuning.TuningPeriodic(); } @Override diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index 412243c..55bd2f1 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -4,10 +4,12 @@ package frc.robot.Subsystems; +import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IntakeConstants; +import frc.robot.Tuning.TuningHelper; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; @@ -21,14 +23,27 @@ public class IntakeSubsystem extends SubsystemBase { private final SparkMax intakeMotor = new SparkMax(IntakeConstants.INTAKE_ID, MotorType.kBrushless); private final SparkMax intakeArmMotor = new SparkMax(IntakeConstants.INTAKE_ARM_ID, MotorType.kBrushless); + private final SparkMax intakeArmMotor2 = new SparkMax(IntakeConstants.INTAKE_ARM_2_ID, MotorType.kBrushless); private final RelativeEncoder intakeArmEncoder = intakeArmMotor.getEncoder(); + private final RelativeEncoder intakeArm2Encoder = intakeArmMotor2.getEncoder(); private final PIDController intakeArmController = new PIDController( IntakeConstants.INTAKE_ARM_kP, IntakeConstants.INTAKE_ARM_kI, IntakeConstants.INTAKE_ARM_kD); + private final PIDController intakeArm2Controller = new PIDController( + IntakeConstants.INTAKE_ARM_kP, + IntakeConstants.INTAKE_ARM_kI, + IntakeConstants.INTAKE_ARM_kD); + + private final ArmFeedforward intakeArmFeedforward = new ArmFeedforward( + IntakeConstants.INTAKE_ARM_kS, + IntakeConstants.INTAKE_ARM_kG, + IntakeConstants.INTAKE_ARM_kV, + IntakeConstants.INTAKE_ARM_kA); + private double intakeArmTargetDeg = IntakeConstants.INTAKE_ARM_RAISED_POSITION; private boolean intakeArmActive = false; @@ -60,6 +75,14 @@ public IntakeSubsystem() { intakeArmController.setTolerance(IntakeConstants.INTAKE_ARM_TOLERANCE_DEG); intakeArmTargetDeg = IntakeConstants.INTAKE_ARM_RAISED_POSITION; intakeArmActive = false; + + intakeArmMotor2.configure(intakeArmConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + //On enable, assume the arm starts raised at 90 degrees + intakeArm2Encoder.setPosition(IntakeConstants.INTAKE_ARM_RAISED_POSITION); + intakeArm2Controller.setTolerance(IntakeConstants.INTAKE_ARM_TOLERANCE_DEG); + intakeArmTargetDeg = IntakeConstants.INTAKE_ARM_RAISED_POSITION; + intakeArmActive = false; + } public void toggleIntake() { @@ -97,6 +120,7 @@ public void setIntakeArmAngle(IntakeArmAngle angle){ IntakeConstants.INTAKE_ARM_MIN_DEG, Math.min(IntakeConstants.INTAKE_ARM_MAX_DEG, intakeArmTargetDeg)); intakeArmController.reset(); + intakeArm2Controller.reset(); intakeArmActive = true; } @@ -121,26 +145,48 @@ public double getArmPositionDeg() { return intakeArmEncoder.getPosition(); } + public double getArmPositionDeg2() { + return intakeArm2Encoder.getPosition(); + } + @Override public void periodic() { double currentDeg = getArmPositionDeg(); + double currentDeg2 = getArmPositionDeg2(); SmartDashboard.putNumber("IntakeArm/TargetDeg", intakeArmTargetDeg); SmartDashboard.putNumber("IntakeArm/PostionDeg", currentDeg); + SmartDashboard.putNumber("IntakeArm/PostionDeg2", currentDeg2); SmartDashboard.putBoolean("IntakeArm/Active", intakeArmActive); + double rawffOutput = intakeArmFeedforward.calculate(Math.toRadians(currentDeg), 0); + double rawffOutput2 = intakeArmFeedforward.calculate(Math.toRadians(currentDeg2), 0); + //divide ff output(in volts) by battery volts for percent output that motor.set expects + double ffOutput = rawffOutput/12; + double ffOutput2 = rawffOutput2/12; + if (intakeArmActive){ - double output = intakeArmController.calculate(currentDeg, intakeArmTargetDeg); + double pidOutput = intakeArmController.calculate(currentDeg, intakeArmTargetDeg); + double pidOutput2 = intakeArm2Controller.calculate(currentDeg2, intakeArmTargetDeg); + + double output = pidOutput + ffOutput; + double output2 = pidOutput2 + ffOutput2; + output = Math.max(IntakeConstants.INTAKE_ARM_MIN_OUTPUT, Math.min(IntakeConstants.INTAKE_ARM_MAX_OUTPUT, output)); + output2 = Math.max(IntakeConstants.INTAKE_ARM_MIN_OUTPUT, Math.min(IntakeConstants.INTAKE_ARM_MAX_OUTPUT, output2)); + - if (intakeArmController.atSetpoint()){ - intakeArmMotor.set(0.0); + if (intakeArmController.atSetpoint() && intakeArm2Controller.atSetpoint()){ + intakeArmMotor.set(ffOutput); + intakeArmMotor2.set(ffOutput2); intakeArmActive = false; } else { intakeArmMotor.set(output); + intakeArmMotor2.set(output2); } } else{ - intakeArmMotor.set(0.0); + intakeArmMotor.set(ffOutput); + intakeArmMotor2.set(ffOutput2); } } } diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 799c565..a5d1853 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -107,11 +107,12 @@ public void simulationReset() { private void configurePathPlanner(){ - AutoBuilder.configure(this::getPose, + AutoBuilder.configure( + this::getPose, this::resetOdometry, this::getChassisSpeeds, - (speeds, feedforwards)->driveFromChassisSpeeds(speeds, false), - AutoConstants.SWERV_DRIVE_CONTROLLER, + (speeds, feedforwards)-> driveFromChassisSpeeds(speeds, false), + AutoConstants.SWERVE_DRIVE_CONTROLLER, AutoConstants.ROBOT_CONFIG, FieldConstants::isRedAlliance, this); @@ -123,7 +124,8 @@ public Command autoDrive(String filename){ if (AutoConstants.isRightSideAuto()){ path = path.mirrorPath(); } - return new FollowPathCommand(path, + return new FollowPathCommand( + path, this::getPose, this::getChassisSpeeds, (speeds, feedforwards) -> driveFromChassisSpeeds(speeds, isVisionEnabled()), @@ -159,17 +161,27 @@ private boolean isVisionEnabled() { } private void updateOdometryWithVision (String limelightName){ - boolean doRejectUpdate = false; - LimelightHelpers.SetRobotOrientation(limelightName, odometry.getEstimatedPosition().getRotation().getDegrees(),0,0,0,0,0); - LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName); - if (mt1 == null){ + boolean doRejectUpdate = false; + + //pass raw yaw + rate instead of estimate here for accuracy + LimelightHelpers.SetRobotOrientation(limelightName, getYaw().getDegrees(),pigeon.getAngularVelocityZWorld().getValueAsDouble(),0,0,0,0); + LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName); + + if (mt2 == null){ return; } if(Math.abs(pigeon.getAngularVelocityZWorld().getValueAsDouble()) > VisionConstants.MAX_VISION_ANGULAR_RATE_DEG_PER_SEC) { doRejectUpdate = true; } - if(mt1.tagCount == 0) + + //reject poses that aren't inside the field + boolean isInField = mt2.pose.getX() > 0 && mt2.pose.getX() < FieldConstants.FIELD_LENGTH + && mt2.pose.getY() > 0 && mt2.pose.getY() < FieldConstants.FIELD_WIDTH; + //reject poses if LL is too far away from tag + boolean isCloseEnough = mt2.avgTagDist < 10; //10 is high, check what we think distance should be + + if(mt2.tagCount <= 0 || isInField == false || isCloseEnough == false) { doRejectUpdate = true; } @@ -181,13 +193,14 @@ private void updateOdometryWithVision (String limelightName){ VisionConstants.VISION_STD_DEV_Y_METERS, VisionConstants.VISION_STD_DEV_THETA_RADIANS)); // need to measure odometry.addVisionMeasurement( - mt1.pose, - mt1.timestampSeconds); + mt2.pose, + mt2.timestampSeconds); } - SmartDashboard.putNumber("Vision/" + limelightName + "/TagCount", mt1.tagCount); - SmartDashboard.putNumber("Vision/" + limelightName + "/AvgTagDist", mt1.avgTagDist); - SmartDashboard.putNumber("Vision/" + limelightName + "/LatencyMs", mt1.latency); + SmartDashboard.putBoolean("Is Vision Good", !doRejectUpdate); + SmartDashboard.putNumber("Vision/" + limelightName + "/TagCount", mt2.tagCount); + SmartDashboard.putNumber("Vision/" + limelightName + "/AvgTagDist", mt2.avgTagDist); + SmartDashboard.putNumber("Vision/" + limelightName + "/LatencyMs", mt2.latency); } @@ -311,6 +324,7 @@ public void periodic() { } } } + //sends robot pose to network tables field.setRobotPose(getPose()); SmartDashboard.putNumber("Pigeon Yaw", pigeon.getYaw().getValueAsDouble()); diff --git a/src/main/java/frc/robot/Tuning/TuningHelper.java b/src/main/java/frc/robot/Tuning/TuningHelper.java new file mode 100644 index 0000000..0b9ea71 --- /dev/null +++ b/src/main/java/frc/robot/Tuning/TuningHelper.java @@ -0,0 +1,59 @@ +package frc.robot.Tuning; + +import java.time.Period; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class TuningHelper { + + /* PID Tuning Variables */ + public static double tuningkP; + public static double tuningkI; + public static double tuningkD; + + /* Feedforward Tuning Variables */ + public static double tuningkS; + public static double tuningkG; + public static double tuningkV; + public static double tuningkA; + + /* Motor Speed Tuning */ + public static double tuningSpeed; + + /* Setpoint Tuning */ + public static double tuningSetpoint; + + /* tuningDeadband */ + public static double tuningDeadband; + + public TuningHelper() { + SmartDashboard.putNumber("Tuning/kP", tuningkP); + SmartDashboard.putNumber("Tuning/kI", tuningkI); + SmartDashboard.putNumber("Tuning/kD", tuningkD); + + SmartDashboard.putNumber("Tuning/kS", tuningkS); + SmartDashboard.putNumber("Tuning/kG", tuningkG); + SmartDashboard.putNumber("Tuning/kV", tuningkV); + SmartDashboard.putNumber("Tuning/kA", tuningkA); + + SmartDashboard.putNumber("Tuning/Speed", tuningSpeed); + SmartDashboard.putNumber("Tuning/Setpoint", tuningSetpoint); + SmartDashboard.putNumber("Tuning/Deadband", tuningDeadband); + } + + public void TuningPeriodic() { + tuningkP = SmartDashboard.getNumber("Tuning/kP", tuningkP); + tuningkI = SmartDashboard.getNumber("Tuning/kI", tuningkI); + tuningkD = SmartDashboard.getNumber("Tuning/kD", tuningkD); + + tuningkS = SmartDashboard.getNumber("Tuning/kS", tuningkS); + tuningkG = SmartDashboard.getNumber("Tuning/kG", tuningkG); + tuningkV = SmartDashboard.getNumber("Tuning/kV", tuningkV); + tuningkA = SmartDashboard.getNumber("Tuning/kA", tuningkA); + + tuningSpeed = SmartDashboard.getNumber("Tuning/Speed", tuningSpeed); + tuningSetpoint = SmartDashboard.getNumber("Tuning/Setpoint", tuningSetpoint); + tuningDeadband = SmartDashboard.getNumber("Tuning/Deadband", tuningDeadband); + } + +} From 3ea7294bcae52ee563c153515609184626307b18 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Sat, 21 Mar 2026 16:50:30 -0500 Subject: [PATCH 33/96] adjust auto --- .../frc/robot/Auto/LeftNeutralZoneAuto1.java | 2 +- .../frc/robot/Auto/RightNeutralZoneAuto1.java | 52 +++++++++++++------ 2 files changed, 36 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index efa69a8..fe9ddf6 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -32,7 +32,7 @@ public class LeftNeutralZoneAuto1 extends SequentialCommandGroup { private static final double FORWARD_METERS_2 = 1.; private static final double FORWARD_METERS_3 = 3.2; - private static final double INTAKE_POWER = -1.0; + private static final double INTAKE_POWER = -0.5; public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addRequirements(drive, intake, shooter); diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java index 0d79712..91dce4f 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.Subsystems.SwerveSubsystem; @@ -29,9 +30,9 @@ public class RightNeutralZoneAuto1 extends SequentialCommandGroup { private static final double BACKWARD_METERS_2 = 3.0; private static final double FORWARD_METERS_1 = 3.0; private static final double FORWARD_METERS_2 = 1.; - private static final double FORWARD_METERS_3 = 3.2; + private static final double FORWARD_METERS_3 = 3.25; - private static final double INTAKE_POWER = -1.0; + private static final double INTAKE_POWER = -0.7; public RightNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addRequirements(drive, intake, shooter); @@ -66,13 +67,13 @@ public RightNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoo Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), // Turn 90 degrees left - turnRelativeDegrees(drive, 90.0), + turnRelativeDegrees(drive, -90.0), // Drive backward (back to the trench) - driveDistanceMeters(drive, -BACKWARD_METERS_2, DRIVE_SPEED_MPS), + driveDistanceMeters(drive,BACKWARD_METERS_2, DRIVE_SPEED_MPS), // Turn 20 degrees right - turnRelativeDegrees(drive, -20.0), + turnRelativeDegrees(drive,-13.0), // Bring hood up to HIGH angle. Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), @@ -84,18 +85,35 @@ public RightNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoo }, shooter), Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) - .withTimeout(2.0), - - // Start kicker first, then start indexer 1 second later (kicker keeps running). - Commands.sequence( - Commands.run(() -> { - shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); - shooter.setIndexerSpeed(0.0); - }, shooter).withTimeout(1.0), - Commands.run(() -> { - shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); - shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); - }, shooter) + .withTimeout(1.0), + + Commands.parallel( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ), + + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + + // While shooting/indexing, continuously move the intake arm up/down. + Commands.sequence( + Commands.runOnce(intake::lowerIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), + Commands.runOnce(intake::raiseIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) + ) + .repeatedly() ) ); } From 97a429ba442b59f96811d72d15d20ed05caf409c Mon Sep 17 00:00:00 2001 From: Gavin Date: Sat, 21 Mar 2026 19:27:42 -0500 Subject: [PATCH 34/96] Pre10k odometry update (#29) * fixed spelling of variable, added some safeguards for limelights, change back to megatag 2, lowered standard deviation for translation, updated updateOdometryWithVision method * Added tuningHelper class * changed tuning variables to only work in test mode * updated spelling mistake in gitignore for ctre_sim folder * added second intake arm motor (will probably need to be fixed) * added feedforward for intake arm * fixed feedforward (whoops but is okay now) * Add the id for the 2nd intake arm motor * Intake arm 2 * intake arm 2 tuning * added a simple auto align * added SimpleAutoAlign to drive controller * changed SimpleAutoAlign PID temporarly to regular swerve PID * update intake speed * auto align for id 25, 8, 11, 24, 27 * auto align distance update --------- Co-authored-by: Nonochen0104 Co-authored-by: Lilly <113945482+raininglilly@users.noreply.github.com> --- .../frc/robot/Command/SimpleAutoAlign.java | 104 ++++++++++++++++++ src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 7 +- 3 files changed, 111 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/Command/SimpleAutoAlign.java diff --git a/src/main/java/frc/robot/Command/SimpleAutoAlign.java b/src/main/java/frc/robot/Command/SimpleAutoAlign.java new file mode 100644 index 0000000..76270a9 --- /dev/null +++ b/src/main/java/frc/robot/Command/SimpleAutoAlign.java @@ -0,0 +1,104 @@ +package frc.robot.Command; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Subsystems.SwerveSubsystem; + +public class SimpleAutoAlign extends Command { + + private SwerveSubsystem swerveSubsystem; + + //target for how far away robot should be from hub + private final double targetDistance = 2.1; //in meters - temporary need to check + + private final double cameraHeight = 0.5; //need to measure + private final double aprilTagHeight = 1; //need to measure + private final double cameraTilt = 0.001; //so math does not end up dividing by 0 + + private final PIDController distanceController = new PIDController(0.1,0,0); //tune this + private final PIDController rotationController = new PIDController(0.01,0,0); //tune this + + public SimpleAutoAlign(SwerveSubsystem swerveSubsystem){ + this.swerveSubsystem = swerveSubsystem; + addRequirements(swerveSubsystem); + } + + private boolean canSeeTag() { + double tv = NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("tv").getDouble(0.0); + return tv > 0; + } + + private int getTagId() { + double tid = NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("tid").getDouble(0.0); + int tidInt = (int) tid; + return tidInt; + } + + private boolean isSupportedTag(int tagId) { + return tagId == 10 || tagId == 25 || tagId == 11 || tagId == 27 || tagId == 8 || tagId == 24; + } + + private double getDesiredAlignmentAngle(int tagId) { + if (tagId == 11 || tagId == 27) { + return 20.0; + } + if (tagId == 8 || tagId == 24) { + return -20.0; + } + return 0.0; + } + + private double getDistanceToTarget() { + double ty = NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("ty").getDouble(0.0); // vertical angle offset in degrees + double angleToTargetRadians = Math.toRadians(cameraTilt + ty); + + double distance = (aprilTagHeight - cameraHeight) / Math.tan(angleToTargetRadians); + return distance; + } + + private double getOffsetToTarget() { + double xDist = NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("tx").getDouble(0.0); + return xDist; + } + + @Override + public void initialize() { + distanceController.reset(); + rotationController.reset(); + } + + @Override + public void execute() { + + double distance = getDistanceToTarget(); + //when i say offset i mean rotation offset + double offset = getOffsetToTarget(); + int tagId = getTagId(); + + //only auto align if distance is valid and can see tag 10 + if (!canSeeTag() || !isSupportedTag(tagId) || distance < 0) { + swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(0,0,0), false); + return; + } + + double desiredAlignmentAngle = getDesiredAlignmentAngle(tagId); + double driveSpeed = distanceController.calculate(distance, targetDistance); + double rotationSpeed = rotationController.calculate(offset, desiredAlignmentAngle); + + swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(driveSpeed, 0, rotationSpeed), false); + + } + + @Override + public void end(boolean interrupted) { + swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(0,0,0), false); + } + + @Override + public boolean isFinished() { + return false; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 58712f5..3684624 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -315,7 +315,7 @@ public static final class IntakeConstants { // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. public static int INTAKE_ID = 19; // SparkMax.set(...) expects [-1.0, 1.0] percent output. - public static double INTAKE_SPEED = 0.90; // max percent output for intake motor + public static double INTAKE_SPEED = 0.50; // max percent output for intake motor public static int INTAKE_ARM_ID = 18; public static int INTAKE_ARM_2_ID = 24; @@ -332,7 +332,7 @@ public static final class IntakeConstants { //PID constants for intake arm (degrees). public static final double INTAKE_ARM_kP = 9.90; public static final double INTAKE_ARM_kI = 1.60; - public static final double INTAKE_ARM_kD = 0.70; + public static final double INTAKE_ARM_kD = 0.16; public static final double INTAKE_ARM_TOLERANCE_DEG = 2.0; //Feedforward constants for intake arm diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cca442d..02fac50 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,6 +33,7 @@ import frc.robot.Constants.ShooterConstants; import frc.robot.Command.AltAutoAlign; import frc.robot.Command.AutoAlign; +import frc.robot.Command.SimpleAutoAlign; import frc.robot.Command.TeleopSwerve; import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.ShooterSubsystem; @@ -188,8 +189,10 @@ private void configureBindings() { driveController.axisGreaterThan(Axis.kLeftTrigger.value, 0.1).whileTrue(new AutoAlign(m_drive, true)); // Right Trigger = Auto-align to right scoring position driveController.axisGreaterThan(Axis.kRightTrigger.value, 0.1).whileTrue(new AutoAlign(m_drive, false)); - // Right Bumper = Alt-Auto-Align - driveController.button(Button.kRightBumper.value).whileTrue(new AltAutoAlign(m_drive, m_shooter)); + // Left Bumper = Alt-Auto-Align + driveController.button(Button.kLeftBumper.value).whileTrue(new AltAutoAlign(m_drive, m_shooter)); + //Right Bumper = Simple Auto Align + driveController.button(Button.kRightBumper.value).whileTrue(new SimpleAutoAlign(m_drive)); // Default command runs continuously when no other command requires the subsystem. // It automatically pauses when commands like AutoAlign take control, then resumes From db9eec51b6f43d4d6381402e15be5933d47d459a Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Sun, 22 Mar 2026 19:33:40 -0500 Subject: [PATCH 35/96] line 161 typo correction --- src/main/java/frc/robot/Subsystems/ShooterSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index 4f78c76..92829fa 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -158,7 +158,7 @@ public void AutoToggleShoot (boolean AutoShootOn) { public void AutoToggleKickIndex (boolean AutoIndexKickOn) { setKickerSpeed(AutoIndexKickOn ? 0 : ShooterConstants.KICKER_SPEED); - setIndexerSpeed(AutoIndexKickOn ? 0 : ShooterConstants.SHOOTER_SPEED); + setIndexerSpeed(AutoIndexKickOn ? 0 : ShooterConstants.INDEXER_SPEED); } @Override From fa033dc2969ed5aec50384d63ba89ebae8dadbcf Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Mon, 23 Mar 2026 16:19:38 -0500 Subject: [PATCH 36/96] Intake arm Hold the intake arm down --- src/main/java/frc/robot/Constants.java | 1 + .../frc/robot/Subsystems/IntakeSubsystem.java | 36 +++++++++++++------ 2 files changed, 26 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3684624..8968f13 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -345,6 +345,7 @@ public static final class IntakeConstants { //duty-cycle / percent output for SparkMax.set(...), which expects a value in [-1.0, 1.0] public static final double INTAKE_ARM_MAX_OUTPUT = 0.20; public static final double INTAKE_ARM_MIN_OUTPUT = -0.10; + public static final double INTAKE_ARM_DOWN_HOLD_OUTPUT = -0.12; } public static final class CANdleConstants { diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index 55bd2f1..1e0b6ff 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -149,6 +149,11 @@ public double getArmPositionDeg2() { return intakeArm2Encoder.getPosition(); } + private boolean isIntakeCommandedDown() { + return Math.abs(intakeArmTargetDeg - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG; + } + @Override public void periodic() { double currentDeg = getArmPositionDeg(); @@ -176,17 +181,26 @@ public void periodic() { output2 = Math.max(IntakeConstants.INTAKE_ARM_MIN_OUTPUT, Math.min(IntakeConstants.INTAKE_ARM_MAX_OUTPUT, output2)); - if (intakeArmController.atSetpoint() && intakeArm2Controller.atSetpoint()){ - intakeArmMotor.set(ffOutput); - intakeArmMotor2.set(ffOutput2); - intakeArmActive = false; - } else { - intakeArmMotor.set(output); - intakeArmMotor2.set(output2); - } - } else{ - intakeArmMotor.set(ffOutput); - intakeArmMotor2.set(ffOutput2); + if (intakeArmController.atSetpoint() && intakeArm2Controller.atSetpoint()) { + if (isIntakeCommandedDown()) { + intakeArmMotor.set(IntakeConstants.INTAKE_ARM_DOWN_HOLD_OUTPUT); + intakeArmMotor2.set(IntakeConstants.INTAKE_ARM_DOWN_HOLD_OUTPUT); + } else { + intakeArmMotor.set(ffOutput); + intakeArmMotor2.set(ffOutput2); + intakeArmActive = false; + } + } else { + intakeArmMotor.set(output); + intakeArmMotor2.set(output2); + } + + } else if (isIntakeCommandedDown()) { + intakeArmMotor.set(IntakeConstants.INTAKE_ARM_DOWN_HOLD_OUTPUT); + intakeArmMotor2.set(IntakeConstants.INTAKE_ARM_DOWN_HOLD_OUTPUT); + } else { + intakeArmMotor.set(ffOutput); + intakeArmMotor2.set(ffOutput2); } } } From 3ec47b7e10f746145d8bda5116b5a025a44e45f3 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Mon, 23 Mar 2026 17:55:59 -0500 Subject: [PATCH 37/96] undo intake holddown --- src/main/java/frc/robot/Constants.java | 1 - .../frc/robot/Subsystems/IntakeSubsystem.java | 36 ++++++------------- 2 files changed, 11 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8968f13..3684624 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -345,7 +345,6 @@ public static final class IntakeConstants { //duty-cycle / percent output for SparkMax.set(...), which expects a value in [-1.0, 1.0] public static final double INTAKE_ARM_MAX_OUTPUT = 0.20; public static final double INTAKE_ARM_MIN_OUTPUT = -0.10; - public static final double INTAKE_ARM_DOWN_HOLD_OUTPUT = -0.12; } public static final class CANdleConstants { diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index 1e0b6ff..55bd2f1 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -149,11 +149,6 @@ public double getArmPositionDeg2() { return intakeArm2Encoder.getPosition(); } - private boolean isIntakeCommandedDown() { - return Math.abs(intakeArmTargetDeg - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) - <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG; - } - @Override public void periodic() { double currentDeg = getArmPositionDeg(); @@ -181,26 +176,17 @@ public void periodic() { output2 = Math.max(IntakeConstants.INTAKE_ARM_MIN_OUTPUT, Math.min(IntakeConstants.INTAKE_ARM_MAX_OUTPUT, output2)); - if (intakeArmController.atSetpoint() && intakeArm2Controller.atSetpoint()) { - if (isIntakeCommandedDown()) { - intakeArmMotor.set(IntakeConstants.INTAKE_ARM_DOWN_HOLD_OUTPUT); - intakeArmMotor2.set(IntakeConstants.INTAKE_ARM_DOWN_HOLD_OUTPUT); - } else { - intakeArmMotor.set(ffOutput); - intakeArmMotor2.set(ffOutput2); - intakeArmActive = false; - } - } else { - intakeArmMotor.set(output); - intakeArmMotor2.set(output2); - } - - } else if (isIntakeCommandedDown()) { - intakeArmMotor.set(IntakeConstants.INTAKE_ARM_DOWN_HOLD_OUTPUT); - intakeArmMotor2.set(IntakeConstants.INTAKE_ARM_DOWN_HOLD_OUTPUT); - } else { - intakeArmMotor.set(ffOutput); - intakeArmMotor2.set(ffOutput2); + if (intakeArmController.atSetpoint() && intakeArm2Controller.atSetpoint()){ + intakeArmMotor.set(ffOutput); + intakeArmMotor2.set(ffOutput2); + intakeArmActive = false; + } else { + intakeArmMotor.set(output); + intakeArmMotor2.set(output2); + } + } else{ + intakeArmMotor.set(ffOutput); + intakeArmMotor2.set(ffOutput2); } } } From 99bde61dcb1e9cc1114a57c391e057d39560e272 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Mon, 23 Mar 2026 20:02:19 -0500 Subject: [PATCH 38/96] auto adjustment --- .../frc/robot/Auto/CenterToDepotAuto.java | 69 ++++++++++++------- .../frc/robot/Auto/LeftNeutralZoneAuto1.java | 2 + src/main/java/frc/robot/Constants.java | 2 +- 3 files changed, 48 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java index 88d5396..e0cf94f 100644 --- a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java +++ b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.Subsystems.IntakeSubsystem; @@ -19,16 +20,19 @@ public class CenterToDepotAuto extends SequentialCommandGroup { private static final double DRIVE_SPEED_MPS = 2; + private static final double DRIVE_SPEED_MPS_2 = 0.8; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; private static final double TURN_TOLERANCE_DEG = 3.0; private static final double TURN_TIMEOUT_SEC = 2.5; - private static final double BACKWARD_METERS_1 = 1; + private static final double BACKWARD_METERS_1 = 1.8; + private static final double FORWARD_METERS = 1.8; private static final double FORWARD_METERS_1 = 2; + private static final double FORWARD_METERS_2 = 2.7; - private static final double INTAKE_POWER = -1.0; + private static final double INTAKE_POWER = -0.75; public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addRequirements(drive, intake, shooter); @@ -42,29 +46,29 @@ public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS // Turn 90 degrees right. turnRelativeDegrees(drive, -90.0), - // Drive forward 2m - driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + // Drive forward 2.7m + driveDistanceMeters(drive, FORWARD_METERS_2, DRIVE_SPEED_MPS), // Turn 90 degrees left (intake on). Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), turnRelativeDegrees(drive, 90.0), - // Drive backwards 1m (intake still on). - driveDistanceMeters(drive, BACKWARD_METERS_1, DRIVE_SPEED_MPS), + // Drive forward 1.8m (intake still on). + driveDistanceMeters(drive, FORWARD_METERS, DRIVE_SPEED_MPS_2), + + // Drive backward 1.8m + driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS_2), // Stop intake at the end. Commands.runOnce(() -> intake.setIntakePower(0.0), intake), Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), - // Turn 90 degrees right (intake still on). - turnRelativeDegrees(drive, -90.0), - - // Drive backward 1m (intake still on). - driveDistanceMeters(drive, BACKWARD_METERS_1, DRIVE_SPEED_MPS), + // Turn 110 degrees right (intake still on). + turnRelativeDegrees(drive, -129), // Bring hood up to HIGH angle. - Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.MED), shooter), // Shooter Commands.runOnce(() -> { @@ -73,18 +77,35 @@ public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS }, shooter), Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) - .withTimeout(2.0), - - // Start kicker first, then start indexer 1 second later (kicker keeps running). - Commands.sequence( - Commands.run(() -> { - shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); - shooter.setIndexerSpeed(0.0); - }, shooter).withTimeout(1.0), - Commands.run(() -> { - shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); - shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); - }, shooter) + .withTimeout(1.0), + + Commands.parallel( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ), + + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + + // While shooting/indexing, continuously move the intake arm up/down. + Commands.sequence( + Commands.runOnce(intake::lowerIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), + Commands.runOnce(intake::raiseIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) + ) + .repeatedly() ) ); } diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index fe9ddf6..26b6f73 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -100,6 +100,8 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot }, shooter) ), + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + // While shooting/indexing, continuously move the intake arm up/down. Commands.sequence( Commands.runOnce(intake::lowerIntake, intake), diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3684624..e633a47 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -315,7 +315,7 @@ public static final class IntakeConstants { // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. public static int INTAKE_ID = 19; // SparkMax.set(...) expects [-1.0, 1.0] percent output. - public static double INTAKE_SPEED = 0.50; // max percent output for intake motor + public static double INTAKE_SPEED = 0.75; // max percent output for intake motor public static int INTAKE_ARM_ID = 18; public static int INTAKE_ARM_2_ID = 24; From 64c9b12a9af668df86688970239cc98a9ced3bf7 Mon Sep 17 00:00:00 2001 From: jmmaloney3 Date: Mon, 23 Mar 2026 20:04:13 -0500 Subject: [PATCH 39/96] velocity compensated intake velocityt proposal --- docs/intake/velocity-compensated-intake.md | 208 +++++++++++++++++++++ 1 file changed, 208 insertions(+) create mode 100644 docs/intake/velocity-compensated-intake.md diff --git a/docs/intake/velocity-compensated-intake.md b/docs/intake/velocity-compensated-intake.md new file mode 100644 index 0000000..ccfff6f --- /dev/null +++ b/docs/intake/velocity-compensated-intake.md @@ -0,0 +1,208 @@ +# Proposal: Velocity-Compensated Intake Roller Speed + +## The Problem + +When our robot drives toward a game piece, the intake rollers sometimes push it away instead of picking it up. This gets worse the faster we drive. + +Here's why: if the robot approaches a piece at 2 m/s but the rollers only spin at a surface speed of 2 m/s, there's zero net "grabbing" force. The rollers need to spin **faster** than the robot is moving to reliably pull pieces in. + +## What Other Teams Do + +A widely referenced guideline (from Team 254, documented on [FRCDesign.org](https://www.frcdesign.org/learning-course/stage2/2C/intake-golden-rules/)) is: + +> Intake roller surface speed should be at least **2x** the robot's ground speed. + +Team 1678 [designed their Rapid React intake](https://www.frcdesign.org/mechanism-examples/intake/linkage/1678_2022_intake/) at 2.5x. The idea is simple: if you might drive at 3 m/s while intaking, your rollers should spin at 6+ m/s surface speed. + +## Where We Stand Today + +With our current settings, the numbers look like this: + +- `INTAKE_SPEED = 0.50` (max 50% motor output) +- Auto uses `INTAKE_POWER = -0.5`, so actual motor output = **25%** +- At 25% output, our roller surface speed is roughly **2.0 m/s** +- Our auto drives at **2.0 m/s** + +That's a **1:1 ratio** -- barely matching robot speed, well below the recommended 2x. This likely explains why intake is unreliable while moving. + +## A Possible Approach + +One option would be to make the intake aware of how fast the robot is driving and automatically set roller speed to maintain the 2x ratio. The operator model would be simplified to: + +- **Stick forward** (past deadband) = intake on. Speed is automatic. +- **Stick backward** (past deadband) = eject at a fixed reverse speed. +- **Stick centered** = intake off. + +The operator doesn't need to modulate roller speed -- the "right" speed is determined by the robot's velocity, not feel. + +### The Core Math + +When the intake is on, the roller output is: + +```java +velocityBasedOutput = (robotForwardMps * INTAKE_SPEED_MULTIPLIER) / ROLLER_MAX_SURFACE_SPEED; +output = MathUtil.clamp(velocityBasedOutput, INTAKE_MIN_SPEED, 1.0); +``` + +Where: +- `robotForwardMps` comes from `SwerveSubsystem.getChassisSpeeds().vxMetersPerSecond` +- `INTAKE_SPEED_MULTIPLIER` is the target multiplier (e.g. 2.0, the minimum recommended by other teams) -- tunable +- `ROLLER_MAX_SURFACE_SPEED` is how fast our rollers spin at 100% output (see below) +- `INTAKE_MIN_SPEED` is the floor -- the slowest the rollers will ever spin when the intake is on (e.g. 0.30). This ensures the rollers still grab pieces when the robot is stationary or moving slowly, since the velocity formula would give near-zero at low speeds. + +The `Math.max` handles the transition naturally: at low robot speeds `INTAKE_MIN_SPEED` applies; once the robot is fast enough that the 2x formula exceeds the minimum, velocity compensation takes over. + +### Constants + +**Tuning constants** (adjust based on testing): +- `INTAKE_MIN_SPEED` -- roller output floor when intake is on (e.g. 0.30) +- `INTAKE_SPEED_MULTIPLIER` -- target roller-to-ground-speed ratio (e.g. 2.0) +- `INTAKE_EJECT_SPEED` -- fixed reverse speed for ejecting + +**Physical parameters** (tweak if the hardware changes): + +```java +public static final double ROLLER_MOTOR_MAX_RPM = 5400; // measure this (see below) +public static final int ROLLER_GEAR_MOTOR_TEETH = 18; // gear on motor shaft +public static final int ROLLER_GEAR_ROLLER_TEETH = 24; // gear on roller shaft +public static final double ROLLER_BASE_DIAMETER = Units.inchesToMeters(1.25); +public static final double ROLLER_SLEEVE_THICKNESS = Units.inchesToMeters(0.125); +``` + +**Computed from the above** (not hardcoded): + +```java +// Gear ratio: how many roller turns per motor turn (18:24 = 0.75) +public static final double ROLLER_GEAR_RATIO = + (double) ROLLER_GEAR_MOTOR_TEETH / ROLLER_GEAR_ROLLER_TEETH; + +// Roller outer diameter including the silicone sleeve on each side +public static final double ROLLER_OUTER_DIAMETER = + ROLLER_BASE_DIAMETER + 2 * ROLLER_SLEEVE_THICKNESS; + +// How far the roller surface travels in one full rotation +public static final double ROLLER_CIRCUMFERENCE = + Math.PI * ROLLER_OUTER_DIAMETER; + +// Roller RPM = motor RPM scaled by gear ratio +public static final double ROLLER_MAX_RPM = + ROLLER_MOTOR_MAX_RPM * ROLLER_GEAR_RATIO; + +// Surface speed (m/s) = roller RPM * circumference, converted from per-minute to per-second +public static final double ROLLER_MAX_SURFACE_SPEED = + ROLLER_MAX_RPM * ROLLER_CIRCUMFERENCE / 60.0; +``` + +Each physical parameter is its own constant so we can verify or adjust them independently. For example, if the silicone sleeve turns out to be thinner than 1/8", just change that one value and everything recalculates. + +### Measuring ROLLER_MOTOR_MAX_RPM + +The `5400` above is a placeholder. To get the real value: + +1. Deploy code, open SmartDashboard +2. Lower the intake arm +3. Hold the intake stick at full power (the rollers don't touch the ground, so just run them) +4. Read the motor RPM from SmartDashboard (the SparkMax encoder reports this) +5. Update the constant with the observed value + +This gives a real-world number that accounts for friction, belt/gear losses, etc. + +## What Would Change in the Code + +Three files would be touched. Auto routines would **not** need any changes. + +### Constants.java + +Add the physical parameter and tuning constants shown above. + +### IntakeSubsystem.java + +**Constructor** -- accept a `DoubleSupplier` so the subsystem can read the robot's forward speed each cycle: + +```java +private final java.util.function.DoubleSupplier forwardSpeedSupplier; +private double requestedIntakePower = 0.0; + +public IntakeSubsystem(java.util.function.DoubleSupplier forwardSpeedSupplier) { + this.forwardSpeedSupplier = forwardSpeedSupplier; + // ... rest of existing constructor unchanged ... +} +``` + +**`setIntakePower()`** -- instead of setting the motor directly, just store what the operator requested. The actual motor command happens in `periodic()`: + +```java +public void setIntakePower(double power) { + // store the request power to be used by periodic() + requestedIntakePower = Math.max(-1.0, Math.min(1.0, power)); + intakeOn = Math.abs(requestedIntakePower) > 0.0; +} +``` + +**`periodic()`** -- at the end, after the existing arm PID logic, add roller motor control. This is where the compensation actually happens: + +```java +// Roller velocity compensation +double output = 0.0; + +if (requestedIntakePower < 0) { + // Intaking: apply velocity compensation + double forwardMps = Math.max(0, forwardSpeedSupplier.getAsDouble()); + + // Output calculated from the robot's current velocity + double velocityBasedOutput = + (forwardMps * IntakeConstants.INTAKE_SPEED_MULTIPLIER) / IntakeConstants.ROLLER_MAX_SURFACE_SPEED; + + // Clamp between INTAKE_MIN_SPEED (so we grab pieces even when stationary) + // and 1.0 (motor can't exceed 100%). Negate because intake direction is negative. + output = -MathUtil.clamp(velocityBasedOutput, IntakeConstants.INTAKE_MIN_SPEED, 1.0); + +} else if (requestedIntakePower > 0) { + // Ejecting: fixed reverse speed + output = IntakeConstants.INTAKE_EJECT_SPEED; +} + +intakeMotor.set(output); +``` + +The key idea: `periodic()` runs every 20ms, so the motor output continuously tracks the robot's speed even though `setIntakePower()` might only be called once (as in auto's `runOnce`). + +### RobotContainer.java + +One-line change to wire the swerve speed into the intake: + +```java +// Before: +private final IntakeSubsystem m_intake = new IntakeSubsystem(); + +// After: +private final IntakeSubsystem m_intake = new IntakeSubsystem( + () -> m_drive.getChassisSpeeds().vxMetersPerSecond); +``` + +This works because `m_drive` is declared before `m_intake`, and the lambda is only called later at runtime. + +## What This Would Look Like With Real Numbers + +Assuming measured max surface speed ~8.1 m/s and `INTAKE_MIN_SPEED = 0.30`: + +Motor output is the result of `max(INTAKE_MIN_SPEED, (robotSpeed * INTAKE_SPEED_MULTIPLIER) / ROLLER_MAX_SURFACE_SPEED)`. Roller-to-robot ratio is roller surface speed / robot speed (the 2x target). + +| Scenario | Robot Speed | Motor Output | Roller Surface Speed | Roller:Robot Ratio | Notes | +|---|---|---|---|---|---| +| Stationary | 0 m/s | 30% | 2.4 m/s | n/a | Min speed applies | +| Slow drive | 1 m/s | 30% | 2.4 m/s | 2.4x | Min speed still higher than 2x formula | +| Auto | 2 m/s | 49% | 4.0 m/s | 2.0x | 2x formula takes over | +| Fast teleop | 3 m/s | 74% | 6.0 m/s | 2.0x | | +| Full speed | 5 m/s | 100% | 8.1 m/s | 1.62x | Mechanical limit | + +## Also Worth Noting + +There's a bug in `toggleIntake()` where the boolean assignments are flipped -- `intakeOn` gets set to `false` when turning on and `true` when turning off. Worth fixing regardless of whether we do velocity compensation. + +## Open Questions for the Team + +- Does the 2x ratio feel right as a starting point, or should we try something different? +- What should `INTAKE_MIN_SPEED` be? 0.30? Higher? +- Are there concerns about current draw from running the rollers harder at high speed? +- Does simplifying the stick to on/off/eject make sense, or do we want to keep variable speed control? From 8f0efbb04b8ac4ace522c733fc7d2cea842b2322 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Mon, 23 Mar 2026 22:18:28 -0500 Subject: [PATCH 40/96] Deleted the intake during auto at the end So the sequence does not call the same subsystem twice at the same time --- .../frc/robot/Auto/CenterToDepotAuto.java | 26 +++++++++---------- .../frc/robot/Auto/LeftNeutralZoneAuto1.java | 2 -- .../frc/robot/Auto/RightNeutralZoneAuto1.java | 2 -- 3 files changed, 12 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java index e0cf94f..3a35ea9 100644 --- a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java +++ b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java @@ -92,20 +92,18 @@ public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS }, shooter) ), - Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), - - // While shooting/indexing, continuously move the intake arm up/down. - Commands.sequence( - Commands.runOnce(intake::lowerIntake, intake), - Commands.waitUntil(() -> - Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) - <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), - Commands.runOnce(intake::raiseIntake, intake), - Commands.waitUntil(() -> - Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) - <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) - ) - .repeatedly() + // While shooting/indexing, continuously move the intake arm up/down. + Commands.sequence( + Commands.runOnce(intake::lowerIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), + Commands.runOnce(intake::raiseIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) + ) + .repeatedly() ) ); } diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index 26b6f73..fe9ddf6 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -100,8 +100,6 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot }, shooter) ), - Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), - // While shooting/indexing, continuously move the intake arm up/down. Commands.sequence( Commands.runOnce(intake::lowerIntake, intake), diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java index 91dce4f..5638f1d 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java @@ -100,8 +100,6 @@ public RightNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoo }, shooter) ), - Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), - // While shooting/indexing, continuously move the intake arm up/down. Commands.sequence( Commands.runOnce(intake::lowerIntake, intake), From 859249911e24e31cbb84edcaf8efedd19fcbd25f Mon Sep 17 00:00:00 2001 From: jmmaloney3 Date: Mon, 23 Mar 2026 23:28:46 -0500 Subject: [PATCH 41/96] updated Where We Stand Today section to make it more clear --- docs/intake/velocity-compensated-intake.md | 37 ++++++++++++++++++---- 1 file changed, 31 insertions(+), 6 deletions(-) diff --git a/docs/intake/velocity-compensated-intake.md b/docs/intake/velocity-compensated-intake.md index ccfff6f..42c825d 100644 --- a/docs/intake/velocity-compensated-intake.md +++ b/docs/intake/velocity-compensated-intake.md @@ -16,14 +16,39 @@ Team 1678 [designed their Rapid React intake](https://www.frcdesign.org/mechanis ## Where We Stand Today -With our current settings, the numbers look like this: +### Assumptions -- `INTAKE_SPEED = 0.50` (max 50% motor output) -- Auto uses `INTAKE_POWER = -0.5`, so actual motor output = **25%** -- At 25% output, our roller surface speed is roughly **2.0 m/s** -- Our auto drives at **2.0 m/s** +The estimates below depend on the following values: -That's a **1:1 ratio** -- barely matching robot speed, well below the recommended 2x. This likely explains why intake is unreliable while moving. +1. Auto commands `DRIVE_SPEED_MPS = 2` (`LeftNeutralZoneAuto1`, line 22). We assume the robot actually drives at approximately 2.0 m/s, which is good enough for the rough calculations in this section. + +2. Roller surface speed can be estimated from physical parameters: + - Gear ratio: 18-tooth motor gear / 24-tooth roller gear = 0.75 reduction + - Roller OD: 1.25" base + 2 × ~1/8" silicone sleeve ≈ 1.5" (0.0381 m) + - Motor: NEO Vortex (published free speed 6784 RPM). Under load we estimate ~5400 RPM. + - Max roller surface speed: `5400 × 0.75 × π × 0.0381 / 60 ≈ 8.1 m/s` + +### Current Auto Behavior + +The following two constants are used to calculate the intake motor output used in `LeftNeutralZoneAuto1.java`: + +The max percent output for the intake motor is specified on line 318 in `Constants.java`: + +```java +INTAKE_SPEED = 0.50; +``` + +The intake power requested during auto is specified on line 35 of `LeftNeutralZoneAuto1.java`: +```java +INTAKE_POWER = -0.5; // negative means intake direction (positive would eject) +``` + +On lines 50 and 103 in `LeftNeutralZoneAuto1.java`, the auto routine calls `IntakeSubsystem.setIntakePower(double power)` with `power` argument equal to `INTAKE_POWER`. This sets the motor output to the following: +```java +INTAKE_POWER * INTAKE_SPEED = (-0.5) * 0.5 = -0.25 +``` + +So actual motor output during auto is **25%**, giving a roller surface speed of roughly `8.1 × 0.25 ≈` **2.0 m/s**. Our auto drives at roughly **2.0 m/s** (see assumptions above), so that's approximately a **1:1 ratio** -- barely matching robot speed and well below the recommended 2x. This likely explains why intake is unreliable while moving. ## A Possible Approach From d595d2c07c9095a28b423bad9f7870cddd6c5b48 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Tue, 24 Mar 2026 22:14:37 -0500 Subject: [PATCH 42/96] Add auto & make intake arm down movement more gently Added run intake for all auto needed it without having the error. Added auto that shoots at the depot. For the CenterToDepotAuto, added a second time that the robot runs into the depot to intake. For the intake arm, when it comes down it was hitting the bumpers too hard so try to use INTAKE_ARM_LOWERING_MIN_OUTPUT to limit it in constants. NEED TESTING. --- .../frc/robot/Auto/CenterToDepotAuto.java | 12 ++ .../frc/robot/Auto/DepotShootingAuto.java | 195 ++++++++++++++++++ .../frc/robot/Auto/LeftNeutralZoneAuto1.java | 3 + .../frc/robot/Auto/RightNeutralZoneAuto1.java | 3 + src/main/java/frc/robot/Constants.java | 6 +- src/main/java/frc/robot/RobotContainer.java | 2 + .../frc/robot/Subsystems/IntakeSubsystem.java | 7 + 7 files changed, 227 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/Auto/DepotShootingAuto.java diff --git a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java index 3a35ea9..021a40e 100644 --- a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java +++ b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java @@ -31,6 +31,8 @@ public class CenterToDepotAuto extends SequentialCommandGroup { private static final double FORWARD_METERS = 1.8; private static final double FORWARD_METERS_1 = 2; private static final double FORWARD_METERS_2 = 2.7; + private static final double FORWARD_METERS_3 = 1; + private static final double INTAKE_POWER = -0.75; @@ -56,6 +58,13 @@ public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS // Drive forward 1.8m (intake still on). driveDistanceMeters(drive, FORWARD_METERS, DRIVE_SPEED_MPS_2), + // Out of depot and then back in (2nd in) + // Drive backward 1m + driveDistanceMeters(drive, -FORWARD_METERS_3, DRIVE_SPEED_MPS), + + // Drive forward 1m + driveDistanceMeters(drive, FORWARD_METERS_3, DRIVE_SPEED_MPS_2), + // Drive backward 1.8m driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS_2), @@ -78,6 +87,9 @@ public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) .withTimeout(1.0), + + // Keep intake running while the intake arm cycles up/down during shooting. + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), Commands.parallel( // Start kicker first, then start indexer 1 second later (kicker keeps running). diff --git a/src/main/java/frc/robot/Auto/DepotShootingAuto.java b/src/main/java/frc/robot/Auto/DepotShootingAuto.java new file mode 100644 index 0000000..83ce056 --- /dev/null +++ b/src/main/java/frc/robot/Auto/DepotShootingAuto.java @@ -0,0 +1,195 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import java.util.concurrent.atomic.AtomicReference; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; +import frc.robot.Subsystems.SwerveSubsystem; + +public class DepotShootingAuto extends SequentialCommandGroup { + private static final double DRIVE_SPEED_MPS = 2; + private static final double DRIVE_SPEED_MPS_2 = 0.8; + private static final double DRIVE_HEADING_P = 3.0; + private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; + private static final double TURN_P = 4.0; + private static final double TURN_TOLERANCE_DEG = 3.0; + private static final double TURN_TIMEOUT_SEC = 2.5; + + private static final double BACKWARD_METERS_1 = 1; + private static final double FORWARD_METERS = 1.8; + private static final double FORWARD_METERS_1 = 2; + private static final double FORWARD_METERS_2 = 2.7; + + private static final double INTAKE_POWER = -0.75; + + public DepotShootingAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + addRequirements(drive, intake, shooter); + + addCommands( + Commands.runOnce(intake::lowerIntake, intake), + + // Drive forwards 2m. + driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Turn 90 degrees right. + turnRelativeDegrees(drive, -90.0), + + // Drive forward 2.7m + driveDistanceMeters(drive, FORWARD_METERS_2, DRIVE_SPEED_MPS), + + // Turn 90 degrees left (intake on). + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + turnRelativeDegrees(drive, 90.0), + + // Drive forward 1.8m (intake still on). + driveDistanceMeters(drive, FORWARD_METERS, DRIVE_SPEED_MPS_2), + + // Drive backward 1m + driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS_2), + + // Stop intake at the end. + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 110 degrees right (intake still on). + turnRelativeDegrees(drive, -129), + + + // Bring hood up to HIGH angle. + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + + // Shooter + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(1.0), + + // Keep intake running while the intake arm cycles up/down during shooting. + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + + Commands.parallel( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + }, shooter) + ), + + // While shooting/indexing, continuously move the intake arm up/down. + Commands.sequence( + Commands.runOnce(intake::lowerIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), + Commands.runOnce(intake::raiseIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) + ) + .repeatedly() + ) + ); + } + + private static edu.wpi.first.wpilibj2.command.Command driveDistanceMeters( + SwerveSubsystem drive, + double distanceMeters, + double speedMps) { + double clampedSpeedMps = MathUtil.clamp(Math.abs(speedMps), 0.0, SwerveConstants.maxSpeed); + double commandedSpeedMps = Math.copySign(clampedSpeedMps, distanceMeters); + double distanceAbsMeters = Math.abs(distanceMeters); + + AtomicReference startPositions = new AtomicReference<>(); + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> { + startPositions.set(drive.getPositions()); + startYawRad[0] = drive.getYaw().getRadians(); + }, drive), + Commands.runEnd( + () -> { + double errorRad = MathUtil.angleModulus(startYawRad[0] - drive.getYaw().getRadians()); + double maxOmegaRadPerSec = + Math.min(DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC, SwerveConstants.maxAngularVelocity); + double omegaRadPerSec = + MathUtil.clamp(errorRad * DRIVE_HEADING_P, -maxOmegaRadPerSec, maxOmegaRadPerSec); + drive.drive(commandedSpeedMps, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until( + () -> getAverageWheelDeltaMeters(startPositions.get(), drive.getPositions()) >= distanceAbsMeters) + .withTimeout(distanceAbsMeters / Math.max(0.1, Math.abs(commandedSpeedMps)) + 1.0) + ); + } + + private static double getAverageWheelDeltaMeters( + SwerveModulePosition[] startPositions, + SwerveModulePosition[] currentPositions) { + if (startPositions == null || currentPositions == null) { + return 0.0; + } + + int count = Math.min(startPositions.length, currentPositions.length); + if (count <= 0) { + return 0.0; + } + + double sum = 0.0; + for (int i = 0; i < count; i++) { + sum += Math.abs(currentPositions[i].distanceMeters - startPositions[i].distanceMeters); + } + return sum / count; + } + + private static edu.wpi.first.wpilibj2.command.Command turnRelativeDegrees( + SwerveSubsystem drive, + double deltaDegrees) { + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.runEnd( + () -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadPerSec = + MathUtil.clamp( + errorRad * TURN_P, + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + drive.drive(0, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(TURN_TOLERANCE_DEG); + }) + .withTimeout(TURN_TIMEOUT_SEC) + + ); + } +} diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index fe9ddf6..3750f64 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -87,6 +87,9 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) .withTimeout(1.0), + // Keep intake running while the intake arm cycles up/down during shooting. + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + Commands.parallel( // Start kicker first, then start indexer 1 second later (kicker keeps running). Commands.sequence( diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java index 5638f1d..3b666fe 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java @@ -86,6 +86,9 @@ public RightNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoo Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) .withTimeout(1.0), + + // Keep intake running while the intake arm cycles up/down during shooting. + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), Commands.parallel( // Start kicker first, then start indexer 1 second later (kicker keeps running). diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e633a47..0b68253 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -162,7 +162,8 @@ public enum AutoMode{ RightNeutralZoneAuto1, ShootEightAuto, CenterLemonAuto, - CenterToDepotAuto + CenterToDepotAuto, + DepotShootingAuto } private static SendableChooser sideChooser = new SendableChooser(); @@ -186,6 +187,7 @@ public static void initDashboard() { autoModeChooser.addOption("LeftNeutralZoneAuto1", AutoMode.LeftNeutralZoneAuto1); autoModeChooser.addOption("CenterLemonAuto", AutoMode.CenterLemonAuto); autoModeChooser.addOption("CenterToDepotAuto", AutoMode.CenterToDepotAuto); + autoModeChooser.addOption("DepotShootingAuto", AutoMode.DepotShootingAuto); SmartDashboard.putData("Auto Starting Location", sideChooser); SmartDashboard.putData("Auto Mode", autoModeChooser); @@ -345,6 +347,8 @@ public static final class IntakeConstants { //duty-cycle / percent output for SparkMax.set(...), which expects a value in [-1.0, 1.0] public static final double INTAKE_ARM_MAX_OUTPUT = 0.20; public static final double INTAKE_ARM_MIN_OUTPUT = -0.10; + // Additional clamp while lowering so the arm descends more softly. + public static final double INTAKE_ARM_LOWERING_MIN_OUTPUT = -0.06; } public static final class CANdleConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 02fac50..a980b17 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,6 +27,7 @@ import frc.robot.Auto.ShootEightAuto; import frc.robot.Auto.CenterLemonAuto; import frc.robot.Auto.CenterToDepotAuto; +import frc.robot.Auto.DepotShootingAuto; import frc.robot.Auto.DriveTestAuto; import frc.robot.Constants.AutoConstants; import frc.robot.Constants.VisionConstants; @@ -250,6 +251,7 @@ public Command getAutonomousCommand() { case ShootEightAuto -> new ShootEightAuto(m_drive, m_intake, m_shooter); case CenterLemonAuto -> new CenterLemonAuto(m_drive, m_intake, m_shooter); case CenterToDepotAuto -> new CenterToDepotAuto(m_drive, m_intake, m_shooter); + case DepotShootingAuto -> new DepotShootingAuto(m_drive, m_intake, m_shooter); default -> Commands.none(); diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index 55bd2f1..c6ddaa1 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -172,6 +172,13 @@ public void periodic() { double output = pidOutput + ffOutput; double output2 = pidOutput2 + ffOutput2; + //NEED TESTING + // When commanded down, limit downward power further so the arm settles more gently. + if (!intakeUp) { + output = Math.max(IntakeConstants.INTAKE_ARM_LOWERING_MIN_OUTPUT, output); + output2 = Math.max(IntakeConstants.INTAKE_ARM_LOWERING_MIN_OUTPUT, output2); + } + output = Math.max(IntakeConstants.INTAKE_ARM_MIN_OUTPUT, Math.min(IntakeConstants.INTAKE_ARM_MAX_OUTPUT, output)); output2 = Math.max(IntakeConstants.INTAKE_ARM_MIN_OUTPUT, Math.min(IntakeConstants.INTAKE_ARM_MAX_OUTPUT, output2)); From 6c6f1d60f9019d1765dd424986f0337730fec675 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 25 Mar 2026 15:08:36 -0500 Subject: [PATCH 43/96] Deleted unused imports --- src/main/java/frc/robot/Auto/CenterToDepotAuto.java | 1 - src/main/java/frc/robot/Auto/DepotShootingAuto.java | 1 - src/main/java/frc/robot/Auto/DriveTestAuto.java | 2 -- 3 files changed, 4 deletions(-) diff --git a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java index 021a40e..6fd7158 100644 --- a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java +++ b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java @@ -9,7 +9,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.ShooterConstants; diff --git a/src/main/java/frc/robot/Auto/DepotShootingAuto.java b/src/main/java/frc/robot/Auto/DepotShootingAuto.java index 83ce056..6cadf6f 100644 --- a/src/main/java/frc/robot/Auto/DepotShootingAuto.java +++ b/src/main/java/frc/robot/Auto/DepotShootingAuto.java @@ -9,7 +9,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.ShooterConstants; diff --git a/src/main/java/frc/robot/Auto/DriveTestAuto.java b/src/main/java/frc/robot/Auto/DriveTestAuto.java index 0f0fa0e..f41b024 100644 --- a/src/main/java/frc/robot/Auto/DriveTestAuto.java +++ b/src/main/java/frc/robot/Auto/DriveTestAuto.java @@ -4,9 +4,7 @@ package frc.robot.Auto; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants.ShooterConstants; import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.ShooterSubsystem; import frc.robot.Subsystems.SwerveSubsystem; From 4b36bffe0365c21ac51c2d64925f80ecfc3c286c Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Wed, 25 Mar 2026 20:44:53 -0500 Subject: [PATCH 44/96] auto update --- .../frc/robot/Auto/CenterToDepotAuto.java | 19 +++++++++++-------- src/main/java/frc/robot/Constants.java | 2 +- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java index 6fd7158..0818abf 100644 --- a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java +++ b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java @@ -18,7 +18,7 @@ import frc.robot.Subsystems.SwerveSubsystem; public class CenterToDepotAuto extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 2; + private static final double DRIVE_SPEED_MPS = 2.5; private static final double DRIVE_SPEED_MPS_2 = 0.8; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; @@ -26,10 +26,10 @@ public class CenterToDepotAuto extends SequentialCommandGroup { private static final double TURN_TOLERANCE_DEG = 3.0; private static final double TURN_TIMEOUT_SEC = 2.5; - private static final double BACKWARD_METERS_1 = 1.8; + private static final double BACKWARD_METERS_1 = 2.3; private static final double FORWARD_METERS = 1.8; private static final double FORWARD_METERS_1 = 2; - private static final double FORWARD_METERS_2 = 2.7; + private static final double FORWARD_METERS_2 = 1.9; private static final double FORWARD_METERS_3 = 1; @@ -47,7 +47,7 @@ public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS // Turn 90 degrees right. turnRelativeDegrees(drive, -90.0), - // Drive forward 2.7m + // Drive forward 1.9m driveDistanceMeters(drive, FORWARD_METERS_2, DRIVE_SPEED_MPS), // Turn 90 degrees left (intake on). @@ -64,7 +64,7 @@ public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS // Drive forward 1m driveDistanceMeters(drive, FORWARD_METERS_3, DRIVE_SPEED_MPS_2), - // Drive backward 1.8m + // Drive backward 2.3m driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS_2), // Stop intake at the end. @@ -72,7 +72,7 @@ public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), // Turn 110 degrees right (intake still on). - turnRelativeDegrees(drive, -129), + turnRelativeDegrees(drive, -125), // Bring hood up to HIGH angle. @@ -86,10 +86,11 @@ public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) .withTimeout(1.0), - + + // Keep intake running while the intake arm cycles up/down during shooting. Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), - + Commands.parallel( // Start kicker first, then start indexer 1 second later (kicker keeps running). Commands.sequence( @@ -103,6 +104,8 @@ public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS }, shooter) ), + Commands.waitSeconds(2), + // While shooting/indexing, continuously move the intake arm up/down. Commands.sequence( Commands.runOnce(intake::lowerIntake, intake), diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0b68253..438dba5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -317,7 +317,7 @@ public static final class IntakeConstants { // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. public static int INTAKE_ID = 19; // SparkMax.set(...) expects [-1.0, 1.0] percent output. - public static double INTAKE_SPEED = 0.75; // max percent output for intake motor + public static double INTAKE_SPEED = 0.5; // max percent output for intake motor public static int INTAKE_ARM_ID = 18; public static int INTAKE_ARM_2_ID = 24; From a8704ed94e8a9854bba823c181dc3f4e2ff69f39 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Thu, 26 Mar 2026 12:41:23 -0500 Subject: [PATCH 45/96] auto --- src/main/java/frc/robot/Auto/CenterToDepotAuto.java | 2 +- src/main/java/frc/robot/Auto/DepotShootingAuto.java | 6 ++++-- src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java | 2 ++ src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java | 2 ++ 4 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java index 0818abf..651cede 100644 --- a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java +++ b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java @@ -18,7 +18,7 @@ import frc.robot.Subsystems.SwerveSubsystem; public class CenterToDepotAuto extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 2.5; + private static final double DRIVE_SPEED_MPS = 2; private static final double DRIVE_SPEED_MPS_2 = 0.8; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; diff --git a/src/main/java/frc/robot/Auto/DepotShootingAuto.java b/src/main/java/frc/robot/Auto/DepotShootingAuto.java index 6cadf6f..13a9a35 100644 --- a/src/main/java/frc/robot/Auto/DepotShootingAuto.java +++ b/src/main/java/frc/robot/Auto/DepotShootingAuto.java @@ -29,7 +29,7 @@ public class DepotShootingAuto extends SequentialCommandGroup { private static final double BACKWARD_METERS_1 = 1; private static final double FORWARD_METERS = 1.8; private static final double FORWARD_METERS_1 = 2; - private static final double FORWARD_METERS_2 = 2.7; + private static final double FORWARD_METERS_2 = 1.9; private static final double INTAKE_POWER = -0.75; @@ -45,7 +45,7 @@ public DepotShootingAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS // Turn 90 degrees right. turnRelativeDegrees(drive, -90.0), - // Drive forward 2.7m + // Drive forward 1.9m driveDistanceMeters(drive, FORWARD_METERS_2, DRIVE_SPEED_MPS), // Turn 90 degrees left (intake on). @@ -94,6 +94,8 @@ public DepotShootingAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS }, shooter) ), + Commands.waitSeconds(2), + // While shooting/indexing, continuously move the intake arm up/down. Commands.sequence( Commands.runOnce(intake::lowerIntake, intake), diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index 3750f64..2d6a44a 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -103,6 +103,8 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot }, shooter) ), + Commands.waitSeconds(2), + // While shooting/indexing, continuously move the intake arm up/down. Commands.sequence( Commands.runOnce(intake::lowerIntake, intake), diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java index 3b666fe..0c8bc0a 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java @@ -103,6 +103,8 @@ public RightNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoo }, shooter) ), + Commands.waitSeconds(2), + // While shooting/indexing, continuously move the intake arm up/down. Commands.sequence( Commands.runOnce(intake::lowerIntake, intake), From 925fdd55fcb602fc33721ea6976dd957573abc4f Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Thu, 26 Mar 2026 14:32:04 -0500 Subject: [PATCH 46/96] auto 2 --- .../frc/robot/Auto/LeftNeutralZoneAuto1.java | 17 ++++++++++++----- .../frc/robot/Auto/RightNeutralZoneAuto1.java | 14 +++++++++++--- 2 files changed, 23 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index 2d6a44a..5c28120 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -19,7 +19,7 @@ public class LeftNeutralZoneAuto1 extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 2; + private static final double DRIVE_SPEED_MPS = 2.0; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; @@ -27,10 +27,11 @@ public class LeftNeutralZoneAuto1 extends SequentialCommandGroup { private static final double TURN_TIMEOUT_SEC = 2.5; private static final double BACKWARD_METERS_1 = 3.6; - private static final double BACKWARD_METERS_2 = 3.0; + private static final double BACKWARD_METERS_2 = 3.3; private static final double FORWARD_METERS_1 = 3.0; - private static final double FORWARD_METERS_2 = 1.; + private static final double FORWARD_METERS_2 = 1.0; private static final double FORWARD_METERS_3 = 3.2; + private static final double FORWARD_METERS_4 = 0.5; private static final double INTAKE_POWER = -0.5; @@ -46,8 +47,14 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot // Turn 90 degrees left. turnRelativeDegrees(drive, 90.0), - // Drive forward 3m while starting intake (intake stays on for the rest of auto). + // Drive forward 0.5m while starting intake (intake stays on for the rest of auto). Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + driveDistanceMeters(drive, FORWARD_METERS_4, DRIVE_SPEED_MPS), + + // Backs up 0.5m + driveDistanceMeters(drive, -FORWARD_METERS_4, DRIVE_SPEED_MPS), + + // Return to the rest of the path, drive forward 3.6m driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), // Turn 90 degrees right (intake still on). @@ -103,7 +110,7 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot }, shooter) ), - Commands.waitSeconds(2), + Commands.waitSeconds(1), // While shooting/indexing, continuously move the intake arm up/down. Commands.sequence( diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java index 0c8bc0a..e7891bb 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java @@ -19,7 +19,7 @@ public class RightNeutralZoneAuto1 extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 2; + private static final double DRIVE_SPEED_MPS = 2.0; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; @@ -29,10 +29,11 @@ public class RightNeutralZoneAuto1 extends SequentialCommandGroup { private static final double BACKWARD_METERS_1 = 3.6; private static final double BACKWARD_METERS_2 = 3.0; private static final double FORWARD_METERS_1 = 3.0; - private static final double FORWARD_METERS_2 = 1.; + private static final double FORWARD_METERS_2 = 1.0; private static final double FORWARD_METERS_3 = 3.25; + private static final double FORWARD_METERS_4 = 0.5; - private static final double INTAKE_POWER = -0.7; + private static final double INTAKE_POWER = -1.0; public RightNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addRequirements(drive, intake, shooter); @@ -48,6 +49,13 @@ public RightNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoo // Drive forward 3m while starting intake (intake stays on for the rest of auto). Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + + driveDistanceMeters(drive, FORWARD_METERS_4, DRIVE_SPEED_MPS), + + // Backs up 0.5m + driveDistanceMeters(drive, -FORWARD_METERS_4, DRIVE_SPEED_MPS), + + // Return to the rest of the path, drive forward 3.6m driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), // Turn 90 degrees left (intake still on). From f8affdc6e4201d6c4a61c5e4fe01593adff1f1af Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Thu, 26 Mar 2026 15:22:36 -0500 Subject: [PATCH 47/96] auto change & increased driving speed --- src/main/java/frc/robot/Auto/CenterToDepotAuto.java | 8 ++++---- src/main/java/frc/robot/Auto/DepotShootingAuto.java | 8 ++++---- .../java/frc/robot/Auto/LeftNeutralZoneAuto1.java | 11 ++--------- .../java/frc/robot/Auto/RightNeutralZoneAuto1.java | 12 ++---------- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 5 files changed, 14 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java index 651cede..3fb883c 100644 --- a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java +++ b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java @@ -18,7 +18,7 @@ import frc.robot.Subsystems.SwerveSubsystem; public class CenterToDepotAuto extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 2; + private static final double DRIVE_SPEED_MPS = 3.0; private static final double DRIVE_SPEED_MPS_2 = 0.8; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; @@ -28,12 +28,12 @@ public class CenterToDepotAuto extends SequentialCommandGroup { private static final double BACKWARD_METERS_1 = 2.3; private static final double FORWARD_METERS = 1.8; - private static final double FORWARD_METERS_1 = 2; + private static final double FORWARD_METERS_1 = 2.0; private static final double FORWARD_METERS_2 = 1.9; - private static final double FORWARD_METERS_3 = 1; + private static final double FORWARD_METERS_3 = 1.0; - private static final double INTAKE_POWER = -0.75; + private static final double INTAKE_POWER = -1.0; public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addRequirements(drive, intake, shooter); diff --git a/src/main/java/frc/robot/Auto/DepotShootingAuto.java b/src/main/java/frc/robot/Auto/DepotShootingAuto.java index 13a9a35..e8ebc93 100644 --- a/src/main/java/frc/robot/Auto/DepotShootingAuto.java +++ b/src/main/java/frc/robot/Auto/DepotShootingAuto.java @@ -18,7 +18,7 @@ import frc.robot.Subsystems.SwerveSubsystem; public class DepotShootingAuto extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 2; + private static final double DRIVE_SPEED_MPS = 3.0; private static final double DRIVE_SPEED_MPS_2 = 0.8; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; @@ -26,12 +26,12 @@ public class DepotShootingAuto extends SequentialCommandGroup { private static final double TURN_TOLERANCE_DEG = 3.0; private static final double TURN_TIMEOUT_SEC = 2.5; - private static final double BACKWARD_METERS_1 = 1; + private static final double BACKWARD_METERS_1 = 1.0; private static final double FORWARD_METERS = 1.8; - private static final double FORWARD_METERS_1 = 2; + private static final double FORWARD_METERS_1 = 2.0; private static final double FORWARD_METERS_2 = 1.9; - private static final double INTAKE_POWER = -0.75; + private static final double INTAKE_POWER = -1.0; public DepotShootingAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addRequirements(drive, intake, shooter); diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index 5c28120..3dd2af5 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -19,7 +19,7 @@ public class LeftNeutralZoneAuto1 extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 2.0; + private static final double DRIVE_SPEED_MPS = 3.0; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; @@ -31,9 +31,8 @@ public class LeftNeutralZoneAuto1 extends SequentialCommandGroup { private static final double FORWARD_METERS_1 = 3.0; private static final double FORWARD_METERS_2 = 1.0; private static final double FORWARD_METERS_3 = 3.2; - private static final double FORWARD_METERS_4 = 0.5; - private static final double INTAKE_POWER = -0.5; + private static final double INTAKE_POWER = -1.0; public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addRequirements(drive, intake, shooter); @@ -49,12 +48,6 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot // Drive forward 0.5m while starting intake (intake stays on for the rest of auto). Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), - driveDistanceMeters(drive, FORWARD_METERS_4, DRIVE_SPEED_MPS), - - // Backs up 0.5m - driveDistanceMeters(drive, -FORWARD_METERS_4, DRIVE_SPEED_MPS), - - // Return to the rest of the path, drive forward 3.6m driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), // Turn 90 degrees right (intake still on). diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java index e7891bb..d153f41 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java @@ -19,7 +19,7 @@ public class RightNeutralZoneAuto1 extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 2.0; + private static final double DRIVE_SPEED_MPS = 3.0; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; @@ -27,11 +27,10 @@ public class RightNeutralZoneAuto1 extends SequentialCommandGroup { private static final double TURN_TIMEOUT_SEC = 2.5; private static final double BACKWARD_METERS_1 = 3.6; - private static final double BACKWARD_METERS_2 = 3.0; + private static final double BACKWARD_METERS_2 = 3.3; private static final double FORWARD_METERS_1 = 3.0; private static final double FORWARD_METERS_2 = 1.0; private static final double FORWARD_METERS_3 = 3.25; - private static final double FORWARD_METERS_4 = 0.5; private static final double INTAKE_POWER = -1.0; @@ -49,13 +48,6 @@ public RightNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoo // Drive forward 3m while starting intake (intake stays on for the rest of auto). Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), - - driveDistanceMeters(drive, FORWARD_METERS_4, DRIVE_SPEED_MPS), - - // Backs up 0.5m - driveDistanceMeters(drive, -FORWARD_METERS_4, DRIVE_SPEED_MPS), - - // Return to the rest of the path, drive forward 3.6m driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), // Turn 90 degrees left (intake still on). diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a980b17..1abd5b0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -203,9 +203,9 @@ private void configureBindings() { // SwerveSubsystem - The drive subsystem to control m_drive, // translationSupplier - Forward/backward speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.85, + () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.95, // strafeSupplier - Side-to-side speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.85, + () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.95, // rotationSupplier - Rotation speed () -> -driveController.getRawAxis(rotationAxis) * 0.5, // robotCentricSupplier - Robot-oriented (true) vs field-oriented (false) From b78f51626aa9714a184824f8e8e13d7e2498cb31 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Thu, 26 Mar 2026 15:34:24 -0500 Subject: [PATCH 48/96] x-lock button on B (drive controller) --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../java/frc/robot/Subsystems/SwerveSubsystem.java | 13 +++++++++++++ 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1abd5b0..234a687 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -133,8 +133,8 @@ private void configureBindings() { driveController.button(Button.kBack.value).onTrue(new InstantCommand(()->m_drive.resyncModuleEncoders(), m_drive)); //Start Button (menu) = save current module offsets (DISABLED ONLY, wheels must be straight) driveController.button(Button.kStart.value).onTrue(new InstantCommand(()->m_drive.saveModuleOffsets(), m_drive)); - - + //xLock + driveController.button(Button.kB.value).whileTrue(m_drive.xLockCommand()); // SHOOTER CONTROLLER m_shooter.setDefaultCommand( diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index a5d1853..32f4413 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -228,6 +228,19 @@ public void driveFromChassisSpeeds(ChassisSpeeds driveSpeeds, boolean isOpenLoop } } + private void Lockwheels () { + for(int i = 0; i < mSwerveMods. length; i++){ + SwerveModule module = mSwerveMods [i]; + Rotation2d angle = SwerveConstants.swerveKinematics.getModules() [i].getAngle(); + SwerveModuleState xState = new SwerveModuleState(0, angle); + module. setDesiredState(xState, false) ; + } + } + + public Command xLockCommand ( ) { + return runOnce(() -> Lockwheels()). repeatedly() ; + } + public ChassisSpeeds getChassisSpeeds(){ return SwerveConstants.swerveKinematics.toChassisSpeeds(getStates()); } From 9bf2153f39c22524abd6cc0f36fc0198fb761643 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Thu, 26 Mar 2026 17:59:07 -0500 Subject: [PATCH 49/96] center to depot auto changes and x lock --- .../frc/robot/Auto/CenterToDepotAuto.java | 22 ++++++------------- .../frc/robot/Auto/LeftNeutralZoneAuto1.java | 2 +- .../frc/robot/Auto/RightNeutralZoneAuto1.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 6 ++--- .../frc/robot/Subsystems/SwerveSubsystem.java | 15 +++++++++++++ 5 files changed, 27 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java index 3fb883c..eb0a569 100644 --- a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java +++ b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java @@ -19,14 +19,14 @@ public class CenterToDepotAuto extends SequentialCommandGroup { private static final double DRIVE_SPEED_MPS = 3.0; - private static final double DRIVE_SPEED_MPS_2 = 0.8; + private static final double DRIVE_SPEED_MPS_2 = 1.0; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; private static final double TURN_TOLERANCE_DEG = 3.0; private static final double TURN_TIMEOUT_SEC = 2.5; - private static final double BACKWARD_METERS_1 = 2.3; + private static final double BACKWARD_METERS_1 = 1.9; private static final double FORWARD_METERS = 1.8; private static final double FORWARD_METERS_1 = 2.0; private static final double FORWARD_METERS_2 = 1.9; @@ -39,14 +39,14 @@ public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS addRequirements(drive, intake, shooter); addCommands( - Commands.runOnce(intake::lowerIntake, intake), - // Drive forwards 2m. driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), // Turn 90 degrees right. turnRelativeDegrees(drive, -90.0), + Commands.runOnce(intake::lowerIntake, intake), + // Drive forward 1.9m driveDistanceMeters(drive, FORWARD_METERS_2, DRIVE_SPEED_MPS), @@ -57,23 +57,15 @@ public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS // Drive forward 1.8m (intake still on). driveDistanceMeters(drive, FORWARD_METERS, DRIVE_SPEED_MPS_2), - // Out of depot and then back in (2nd in) - // Drive backward 1m - driveDistanceMeters(drive, -FORWARD_METERS_3, DRIVE_SPEED_MPS), - - // Drive forward 1m - driveDistanceMeters(drive, FORWARD_METERS_3, DRIVE_SPEED_MPS_2), - - // Drive backward 2.3m - driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS_2), + // Drive backward 1.9m + driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS), // Stop intake at the end. Commands.runOnce(() -> intake.setIntakePower(0.0), intake), Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), // Turn 110 degrees right (intake still on). - turnRelativeDegrees(drive, -125), - + turnRelativeDegrees(drive, -123), // Bring hood up to HIGH angle. Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.MED), shooter), diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index 3dd2af5..80e30ab 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -19,7 +19,7 @@ public class LeftNeutralZoneAuto1 extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 3.0; + private static final double DRIVE_SPEED_MPS = 2.0; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java index d153f41..3ca64a5 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java @@ -19,7 +19,7 @@ public class RightNeutralZoneAuto1 extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 3.0; + private static final double DRIVE_SPEED_MPS = 2.0; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 234a687..55c6da3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -134,7 +134,7 @@ private void configureBindings() { //Start Button (menu) = save current module offsets (DISABLED ONLY, wheels must be straight) driveController.button(Button.kStart.value).onTrue(new InstantCommand(()->m_drive.saveModuleOffsets(), m_drive)); //xLock - driveController.button(Button.kB.value).whileTrue(m_drive.xLockCommand()); + driveController.button(Button.kB.value).onTrue(new InstantCommand(() -> m_drive.xLock())); // SHOOTER CONTROLLER m_shooter.setDefaultCommand( @@ -203,9 +203,9 @@ private void configureBindings() { // SwerveSubsystem - The drive subsystem to control m_drive, // translationSupplier - Forward/backward speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.95, + () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 1.0, // strafeSupplier - Side-to-side speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.95, + () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 1.0, // rotationSupplier - Rotation speed () -> -driveController.getRawAxis(rotationAxis) * 0.5, // robotCentricSupplier - Robot-oriented (true) vs field-oriented (false) diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 32f4413..5d2d4e3 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -241,6 +241,21 @@ public Command xLockCommand ( ) { return runOnce(() -> Lockwheels()). repeatedly() ; } + public void xLock() + { + SwerveModuleState[] moduleStates = new SwerveModuleState[4] ; + moduleStates[0] = new SwerveModuleState(0,Rotation2d.fromDegrees(315)); + moduleStates[1] = new SwerveModuleState(0,Rotation2d.fromDegrees(45)); + moduleStates[2] = new SwerveModuleState(0,Rotation2d.fromDegrees(225)); + moduleStates[3] = new SwerveModuleState(0,Rotation2d.fromDegrees(135)); + System.out.println("***************attempting to x lock"); + //setModuleStatesStopped(moduleStates); + + for (SwerveModule mod : mSwerveMods) { + mod.setDesiredState(moduleStates[mod.moduleNumber], true); //NEED CONFIRM + } + } + public ChassisSpeeds getChassisSpeeds(){ return SwerveConstants.swerveKinematics.toChassisSpeeds(getStates()); } From 19b14cec8b73ed8451a5aa7e24e31393b9541619 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Thu, 26 Mar 2026 23:47:59 -0500 Subject: [PATCH 50/96] xlock control update Updated the driver B-button binding from a one-shot onTrue trigger to whileTrue, so X-lock is actively commanded for the entire time the button is held. Made the hold behavior explicitly require the swerve subsystem via Commands.run, so teleop drive is interrupted while B is held and naturally resumes once B is released --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 55c6da3..852ffcb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -134,7 +134,7 @@ private void configureBindings() { //Start Button (menu) = save current module offsets (DISABLED ONLY, wheels must be straight) driveController.button(Button.kStart.value).onTrue(new InstantCommand(()->m_drive.saveModuleOffsets(), m_drive)); //xLock - driveController.button(Button.kB.value).onTrue(new InstantCommand(() -> m_drive.xLock())); + driveController.button(Button.kB.value).whileTrue(Commands.run(() -> m_drive.xLock(), m_drive)); // SHOOTER CONTROLLER m_shooter.setDefaultCommand( From 0d551c504f3742ad6e4c168e2b62f5c3af6011ab Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Thu, 26 Mar 2026 23:58:44 -0500 Subject: [PATCH 51/96] went over changes made today Went over the changes that's been made today, adjust the speed of left&right neutral zone auto to what it was (when it worked at other practice fields). --- src/main/java/frc/robot/Auto/CenterToDepotAuto.java | 5 ++--- src/main/java/frc/robot/Auto/DepotShootingAuto.java | 2 +- src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java | 2 +- src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java | 2 +- 4 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java index eb0a569..2300f84 100644 --- a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java +++ b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java @@ -29,8 +29,7 @@ public class CenterToDepotAuto extends SequentialCommandGroup { private static final double BACKWARD_METERS_1 = 1.9; private static final double FORWARD_METERS = 1.8; private static final double FORWARD_METERS_1 = 2.0; - private static final double FORWARD_METERS_2 = 1.9; - private static final double FORWARD_METERS_3 = 1.0; + private static final double FORWARD_METERS_2 = 1.8; private static final double INTAKE_POWER = -1.0; @@ -57,7 +56,7 @@ public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS // Drive forward 1.8m (intake still on). driveDistanceMeters(drive, FORWARD_METERS, DRIVE_SPEED_MPS_2), - // Drive backward 1.9m + // Drive backward 1.8m driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS), // Stop intake at the end. diff --git a/src/main/java/frc/robot/Auto/DepotShootingAuto.java b/src/main/java/frc/robot/Auto/DepotShootingAuto.java index e8ebc93..707e044 100644 --- a/src/main/java/frc/robot/Auto/DepotShootingAuto.java +++ b/src/main/java/frc/robot/Auto/DepotShootingAuto.java @@ -19,7 +19,7 @@ public class DepotShootingAuto extends SequentialCommandGroup { private static final double DRIVE_SPEED_MPS = 3.0; - private static final double DRIVE_SPEED_MPS_2 = 0.8; + private static final double DRIVE_SPEED_MPS_2 = 1.0; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index 80e30ab..65bbb84 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -32,7 +32,7 @@ public class LeftNeutralZoneAuto1 extends SequentialCommandGroup { private static final double FORWARD_METERS_2 = 1.0; private static final double FORWARD_METERS_3 = 3.2; - private static final double INTAKE_POWER = -1.0; + private static final double INTAKE_POWER = -0.75; public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addRequirements(drive, intake, shooter); diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java index 3ca64a5..5add02c 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java @@ -32,7 +32,7 @@ public class RightNeutralZoneAuto1 extends SequentialCommandGroup { private static final double FORWARD_METERS_2 = 1.0; private static final double FORWARD_METERS_3 = 3.25; - private static final double INTAKE_POWER = -1.0; + private static final double INTAKE_POWER = -0.75; public RightNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addRequirements(drive, intake, shooter); From 9429499f306eee1de26b5cc45213500c2445b48e Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Fri, 27 Mar 2026 10:12:09 -0500 Subject: [PATCH 52/96] auto changes Instead of a rectangle, move backwards. --- .../frc/robot/Auto/CenterToDepotAuto.java | 5 ++-- .../frc/robot/Auto/LeftNeutralZoneAuto1.java | 27 +++++++------------ .../frc/robot/Auto/RightNeutralZoneAuto1.java | 27 ++++++------------- src/main/java/frc/robot/Constants.java | 4 +-- 4 files changed, 22 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java index 2300f84..eb0a569 100644 --- a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java +++ b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java @@ -29,7 +29,8 @@ public class CenterToDepotAuto extends SequentialCommandGroup { private static final double BACKWARD_METERS_1 = 1.9; private static final double FORWARD_METERS = 1.8; private static final double FORWARD_METERS_1 = 2.0; - private static final double FORWARD_METERS_2 = 1.8; + private static final double FORWARD_METERS_2 = 1.9; + private static final double FORWARD_METERS_3 = 1.0; private static final double INTAKE_POWER = -1.0; @@ -56,7 +57,7 @@ public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS // Drive forward 1.8m (intake still on). driveDistanceMeters(drive, FORWARD_METERS, DRIVE_SPEED_MPS_2), - // Drive backward 1.8m + // Drive backward 1.9m driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS), // Stop intake at the end. diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index 65bbb84..f9eb1a7 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -27,10 +27,10 @@ public class LeftNeutralZoneAuto1 extends SequentialCommandGroup { private static final double TURN_TIMEOUT_SEC = 2.5; private static final double BACKWARD_METERS_1 = 3.6; - private static final double BACKWARD_METERS_2 = 3.3; - private static final double FORWARD_METERS_1 = 3.0; - private static final double FORWARD_METERS_2 = 1.0; - private static final double FORWARD_METERS_3 = 3.2; + private static final double BACKWARD_METERS_2 = 4.3; + private static final double FORWARD_METERS_1 = 2.0; + //private static final double FORWARD_METERS_2 = 1.0; + //private static final double FORWARD_METERS_3 = 3.2; private static final double INTAKE_POWER = -0.75; @@ -46,28 +46,19 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot // Turn 90 degrees left. turnRelativeDegrees(drive, 90.0), - // Drive forward 0.5m while starting intake (intake stays on for the rest of auto). + // Drive forward 2m while starting intake (intake stays on for the rest of auto). Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), - // Turn 90 degrees right (intake still on). - turnRelativeDegrees(drive, -90.0), - - // Drive forward 1.3m (intake still on). - driveDistanceMeters(drive, FORWARD_METERS_2, DRIVE_SPEED_MPS), - - // Turn 90 degrees right (intake still on). - turnRelativeDegrees(drive, -90.0), - - // Drive forward 3.4m (intake still on). - driveDistanceMeters(drive, FORWARD_METERS_3, DRIVE_SPEED_MPS), + // Drive forward 2m (intake still on). + driveDistanceMeters(drive, -FORWARD_METERS_1, DRIVE_SPEED_MPS), // Stop intake at the end. Commands.runOnce(() -> intake.setIntakePower(0.0), intake), Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), - // Turn 90 degrees right - turnRelativeDegrees(drive, -90.0), + // Turn 90 degrees left + turnRelativeDegrees(drive, 90.0), // Drive backward (back to the trench) driveDistanceMeters(drive, -BACKWARD_METERS_2, DRIVE_SPEED_MPS), diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java index 5add02c..e17e19f 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java @@ -27,10 +27,8 @@ public class RightNeutralZoneAuto1 extends SequentialCommandGroup { private static final double TURN_TIMEOUT_SEC = 2.5; private static final double BACKWARD_METERS_1 = 3.6; - private static final double BACKWARD_METERS_2 = 3.3; - private static final double FORWARD_METERS_1 = 3.0; - private static final double FORWARD_METERS_2 = 1.0; - private static final double FORWARD_METERS_3 = 3.25; + private static final double BACKWARD_METERS_2 = 4.3; + private static final double FORWARD_METERS_1 = 2.0; private static final double INTAKE_POWER = -0.75; @@ -46,33 +44,24 @@ public RightNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoo // Turn 90 degrees right. turnRelativeDegrees(drive, -90.0), - // Drive forward 3m while starting intake (intake stays on for the rest of auto). + // Drive forward 2m while starting intake (intake stays on for the rest of auto). Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), - // Turn 90 degrees left (intake still on). - turnRelativeDegrees(drive, 90.0), - - // Drive forward 1.3m (intake still on). - driveDistanceMeters(drive, FORWARD_METERS_2, DRIVE_SPEED_MPS), + // Drive back 2m (intake still on). + driveDistanceMeters(drive, -FORWARD_METERS_1, DRIVE_SPEED_MPS), // Turn 90 degrees left (intake still on). turnRelativeDegrees(drive, 90.0), - // Drive forward 3.4m (intake still on). - driveDistanceMeters(drive, FORWARD_METERS_3, DRIVE_SPEED_MPS), - // Stop intake at the end. Commands.runOnce(() -> intake.setIntakePower(0.0), intake), Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), - // Turn 90 degrees left - turnRelativeDegrees(drive, -90.0), - - // Drive backward (back to the trench) - driveDistanceMeters(drive,BACKWARD_METERS_2, DRIVE_SPEED_MPS), + // Drive forward (back to the trench) + driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), - // Turn 20 degrees right + // Turn 13 degrees right turnRelativeDegrees(drive,-13.0), // Bring hood up to HIGH angle. diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 438dba5..24fcb54 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -130,7 +130,7 @@ public record ModuleData( public static ModuleData[] moduleData = { new ModuleData(6, 5, 7, 39.90, FRONT_LEFT, driveInvert, angleInvert), //Mod 0 Front left new ModuleData(9, 8, 10, 46.93, FRONT_RIGHT, driveInvert, angleInvert), //Mod 1 Front right - new ModuleData(12, 11, 13, 42.09, BACK_RIGHT, driveInvert, angleInvert), //Mod 2 Back right + new ModuleData(12, 11, 13, 31.72, BACK_RIGHT, driveInvert, angleInvert), //Mod 2 Back right new ModuleData(15, 14, 16, 7.11, BACK_LEFT, driveInvert, angleInvert) //Mod 3 Back left }; @@ -317,7 +317,7 @@ public static final class IntakeConstants { // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. public static int INTAKE_ID = 19; // SparkMax.set(...) expects [-1.0, 1.0] percent output. - public static double INTAKE_SPEED = 0.5; // max percent output for intake motor + public static double INTAKE_SPEED = 0.75; // max percent output for intake motor public static int INTAKE_ARM_ID = 18; public static int INTAKE_ARM_2_ID = 24; From a6552666a5e223e66abbb25a8890ff74ec53785b Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Fri, 27 Mar 2026 15:40:00 -0500 Subject: [PATCH 53/96] center auto distances --- src/main/java/frc/robot/Auto/CenterToDepotAuto.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java index eb0a569..d54d60f 100644 --- a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java +++ b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java @@ -18,7 +18,7 @@ import frc.robot.Subsystems.SwerveSubsystem; public class CenterToDepotAuto extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 3.0; + private static final double DRIVE_SPEED_MPS = 2.0; private static final double DRIVE_SPEED_MPS_2 = 1.0; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; @@ -26,9 +26,9 @@ public class CenterToDepotAuto extends SequentialCommandGroup { private static final double TURN_TOLERANCE_DEG = 3.0; private static final double TURN_TIMEOUT_SEC = 2.5; - private static final double BACKWARD_METERS_1 = 1.9; + private static final double BACKWARD_METERS_1 = 1.7; private static final double FORWARD_METERS = 1.8; - private static final double FORWARD_METERS_1 = 2.0; + private static final double FORWARD_METERS_1 = 1.8; private static final double FORWARD_METERS_2 = 1.9; private static final double FORWARD_METERS_3 = 1.0; @@ -57,7 +57,7 @@ public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS // Drive forward 1.8m (intake still on). driveDistanceMeters(drive, FORWARD_METERS, DRIVE_SPEED_MPS_2), - // Drive backward 1.9m + // Drive backward 1.7m driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS), // Stop intake at the end. From 2ce74583f4383a98be9e2a2e6b981a81628710cf Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Sat, 28 Mar 2026 10:03:27 -0500 Subject: [PATCH 54/96] center shooting auto fix --- src/main/java/frc/robot/Auto/CenterLemonAuto.java | 2 +- src/main/java/frc/robot/Auto/ShootEightAuto.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Auto/CenterLemonAuto.java b/src/main/java/frc/robot/Auto/CenterLemonAuto.java index 92d19ff..7fe6d5f 100644 --- a/src/main/java/frc/robot/Auto/CenterLemonAuto.java +++ b/src/main/java/frc/robot/Auto/CenterLemonAuto.java @@ -20,7 +20,7 @@ public CenterLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSub Commands.waitSeconds(2), new InstantCommand(()->drive.drive(0,0,0, false),drive), - Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), shooter), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.MED), shooter), Commands.runOnce(() -> { shooter.runIndexer(false); shooter.runKicker(false); diff --git a/src/main/java/frc/robot/Auto/ShootEightAuto.java b/src/main/java/frc/robot/Auto/ShootEightAuto.java index 3ec5bf5..d4dba86 100644 --- a/src/main/java/frc/robot/Auto/ShootEightAuto.java +++ b/src/main/java/frc/robot/Auto/ShootEightAuto.java @@ -16,7 +16,7 @@ public class ShootEightAuto extends SequentialCommandGroup { public ShootEightAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { final double[] startYawRad = new double[1]; addCommands( - Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), shooter), Commands.runOnce(() -> { shooter.runIndexer(false); shooter.runKicker(false); From 414376ca755d86f5a1c3beb4ddbf461f47a83ca2 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Mon, 13 Apr 2026 17:25:51 -0500 Subject: [PATCH 55/96] xlock --- .../frc/robot/Command/SimpleAutoAlign.java | 10 +++---- src/main/java/frc/robot/RobotContainer.java | 7 ----- .../frc/robot/Subsystems/SwerveSubsystem.java | 26 +++++++++---------- 3 files changed, 17 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/robot/Command/SimpleAutoAlign.java b/src/main/java/frc/robot/Command/SimpleAutoAlign.java index 76270a9..68cef10 100644 --- a/src/main/java/frc/robot/Command/SimpleAutoAlign.java +++ b/src/main/java/frc/robot/Command/SimpleAutoAlign.java @@ -13,12 +13,12 @@ public class SimpleAutoAlign extends Command { //target for how far away robot should be from hub private final double targetDistance = 2.1; //in meters - temporary need to check - private final double cameraHeight = 0.5; //need to measure - private final double aprilTagHeight = 1; //need to measure + private final double cameraHeight = 0.5; + private final double aprilTagHeight = 1; private final double cameraTilt = 0.001; //so math does not end up dividing by 0 private final PIDController distanceController = new PIDController(0.1,0,0); //tune this - private final PIDController rotationController = new PIDController(0.01,0,0); //tune this + private final PIDController rotationController = new PIDController(0.1,0,0); //tune this public SimpleAutoAlign(SwerveSubsystem swerveSubsystem){ this.swerveSubsystem = swerveSubsystem; @@ -37,7 +37,7 @@ private int getTagId() { } private boolean isSupportedTag(int tagId) { - return tagId == 10 || tagId == 25 || tagId == 11 || tagId == 27 || tagId == 8 || tagId == 24; + return tagId == 10 || tagId == 9 || tagId == 25 || tagId == 11 || tagId == 27 || tagId == 8 || tagId == 24; } private double getDesiredAlignmentAngle(int tagId) { @@ -87,7 +87,7 @@ public void execute() { double driveSpeed = distanceController.calculate(distance, targetDistance); double rotationSpeed = rotationController.calculate(offset, desiredAlignmentAngle); - swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(driveSpeed, 0, rotationSpeed), false); + swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(0, driveSpeed, rotationSpeed), false); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 852ffcb..59df386 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -185,13 +185,6 @@ private void configureBindings() { helmsController.b().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.MED), m_shooter)); helmsController.a().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), m_shooter)); - - // Left Trigger = Auto-align to left scoring position - driveController.axisGreaterThan(Axis.kLeftTrigger.value, 0.1).whileTrue(new AutoAlign(m_drive, true)); - // Right Trigger = Auto-align to right scoring position - driveController.axisGreaterThan(Axis.kRightTrigger.value, 0.1).whileTrue(new AutoAlign(m_drive, false)); - // Left Bumper = Alt-Auto-Align - driveController.button(Button.kLeftBumper.value).whileTrue(new AltAutoAlign(m_drive, m_shooter)); //Right Bumper = Simple Auto Align driveController.button(Button.kRightBumper.value).whileTrue(new SimpleAutoAlign(m_drive)); diff --git a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java index 5d2d4e3..cc7cd1c 100644 --- a/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/SwerveSubsystem.java @@ -238,22 +238,20 @@ private void Lockwheels () { } public Command xLockCommand ( ) { - return runOnce(() -> Lockwheels()). repeatedly() ; + return run(this::xLock); } - public void xLock() - { - SwerveModuleState[] moduleStates = new SwerveModuleState[4] ; - moduleStates[0] = new SwerveModuleState(0,Rotation2d.fromDegrees(315)); - moduleStates[1] = new SwerveModuleState(0,Rotation2d.fromDegrees(45)); - moduleStates[2] = new SwerveModuleState(0,Rotation2d.fromDegrees(225)); - moduleStates[3] = new SwerveModuleState(0,Rotation2d.fromDegrees(135)); - System.out.println("***************attempting to x lock"); - //setModuleStatesStopped(moduleStates); - - for (SwerveModule mod : mSwerveMods) { - mod.setDesiredState(moduleStates[mod.moduleNumber], true); //NEED CONFIRM - } + public void xLock(){ + Rotation2d[] xLockAngles = new Rotation2d [4]; + xLockAngles[0] = Rotation2d.fromDegrees(225); + xLockAngles[1] = Rotation2d.fromDegrees(135); + xLockAngles[2] = Rotation2d.fromDegrees(225); + xLockAngles[3] = Rotation2d.fromDegrees(135); + + for (SwerveModule mod:mSwerveMods){ + mod.setDesiredState(new SwerveModuleState(0,mod.getState().angle), true); + mod.pointInDirection(xLockAngles[mod.moduleNumber].getDegrees()); + } } public ChassisSpeeds getChassisSpeeds(){ From 68fe4c4adfa5b6a2201a906ea97497381fbde8b7 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Fri, 17 Apr 2026 17:20:34 -0500 Subject: [PATCH 56/96] auto align --- src/main/java/frc/robot/Command/SimpleAutoAlign.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Command/SimpleAutoAlign.java b/src/main/java/frc/robot/Command/SimpleAutoAlign.java index 68cef10..7ab4a37 100644 --- a/src/main/java/frc/robot/Command/SimpleAutoAlign.java +++ b/src/main/java/frc/robot/Command/SimpleAutoAlign.java @@ -17,8 +17,8 @@ public class SimpleAutoAlign extends Command { private final double aprilTagHeight = 1; private final double cameraTilt = 0.001; //so math does not end up dividing by 0 - private final PIDController distanceController = new PIDController(0.1,0,0); //tune this - private final PIDController rotationController = new PIDController(0.1,0,0); //tune this + private final PIDController distanceController = new PIDController(0.05,0,0); //tune this + private final PIDController rotationController = new PIDController(0.02,0,2); //tune this public SimpleAutoAlign(SwerveSubsystem swerveSubsystem){ this.swerveSubsystem = swerveSubsystem; @@ -84,10 +84,10 @@ public void execute() { } double desiredAlignmentAngle = getDesiredAlignmentAngle(tagId); - double driveSpeed = distanceController.calculate(distance, targetDistance); + double driveSpeed = distanceController.calculate(targetDistance, distance); double rotationSpeed = rotationController.calculate(offset, desiredAlignmentAngle); - swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(0, driveSpeed, rotationSpeed), false); + swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(driveSpeed, 0, rotationSpeed), false); } From 87c746d19fac0e768db337396311f8bfe0b92d71 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Fri, 17 Apr 2026 18:02:13 -0500 Subject: [PATCH 57/96] update simple auto align with more constants --- .../frc/robot/Command/SimpleAutoAlign.java | 70 +++++++++++++------ 1 file changed, 48 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/Command/SimpleAutoAlign.java b/src/main/java/frc/robot/Command/SimpleAutoAlign.java index 7ab4a37..1553a1e 100644 --- a/src/main/java/frc/robot/Command/SimpleAutoAlign.java +++ b/src/main/java/frc/robot/Command/SimpleAutoAlign.java @@ -1,5 +1,6 @@ package frc.robot.Command; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.networktables.NetworkTableInstance; @@ -8,17 +9,36 @@ public class SimpleAutoAlign extends Command { - private SwerveSubsystem swerveSubsystem; + private final SwerveSubsystem swerveSubsystem; - //target for how far away robot should be from hub - private final double targetDistance = 2.1; //in meters - temporary need to check + //Target for how far away the robot should be from the hub + private static final double TARGET_DISTANCE_METERS = 2.1; - private final double cameraHeight = 0.5; - private final double aprilTagHeight = 1; - private final double cameraTilt = 0.001; //so math does not end up dividing by 0 + //Camera geometry + public static final double CAMERA_HEIGHT_METERS = 0.5; + public static final double APRIL_TAG_HEIGHT_METERS = 1.0; + public static final double CAMERA_TILT_DEG = 0.000001; + + //Distance PID tuning + public static final double DISTANCE_KP = 0; + public static final double DISTANCE_KI = 0; + public static final double DISTANCE_KD = 0; + + //Rotation PID tuning + public static final double ROTATION_KP = 0; + public static final double ROTATION_KI = 0; + public static final double ROTATION_KD = 0; + + //Tolerance and + public static final double DISTANCE_TOLERANCE_METERS = 0.08; + public static final double ROTATION_TOLERANCE_DEG = 1.5; + public static final double MAX_FORWARD_SPEED_MPS = 1.25; + public static final double MAX_ROTATION_SPEED_RAD_PER_SEC = 2.5; + + private final PIDController distanceController = new PIDController(DISTANCE_KP, DISTANCE_KI, DISTANCE_KD); + private final PIDController rotationController = new PIDController(ROTATION_KP, ROTATION_KI, ROTATION_KD); - private final PIDController distanceController = new PIDController(0.05,0,0); //tune this - private final PIDController rotationController = new PIDController(0.02,0,2); //tune this + public SimpleAutoAlign(SwerveSubsystem swerveSubsystem){ this.swerveSubsystem = swerveSubsystem; @@ -31,9 +51,7 @@ private boolean canSeeTag() { } private int getTagId() { - double tid = NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("tid").getDouble(0.0); - int tidInt = (int) tid; - return tidInt; + return (int) NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("tid").getDouble(0.0); } private boolean isSupportedTag(int tagId) { @@ -52,41 +70,49 @@ private double getDesiredAlignmentAngle(int tagId) { private double getDistanceToTarget() { double ty = NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("ty").getDouble(0.0); // vertical angle offset in degrees - double angleToTargetRadians = Math.toRadians(cameraTilt + ty); - - double distance = (aprilTagHeight - cameraHeight) / Math.tan(angleToTargetRadians); - return distance; + double angleToTargetRadians = Math.toRadians(CAMERA_TILT_DEG + ty); + return (APRIL_TAG_HEIGHT_METERS - CAMERA_HEIGHT_METERS) / Math.tan(angleToTargetRadians); } - private double getOffsetToTarget() { - double xDist = NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("tx").getDouble(0.0); - return xDist; + private double getOffsetToTarget() { + return NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("tx").getDouble(0.0); } @Override public void initialize() { distanceController.reset(); rotationController.reset(); + distanceController.setTolerance(DISTANCE_TOLERANCE_METERS); + rotationController.setTolerance(ROTATION_TOLERANCE_DEG); } @Override public void execute() { - double distance = getDistanceToTarget(); - //when i say offset i mean rotation offset double offset = getOffsetToTarget(); int tagId = getTagId(); //only auto align if distance is valid and can see tag 10 - if (!canSeeTag() || !isSupportedTag(tagId) || distance < 0) { + if (!canSeeTag() || !isSupportedTag(tagId) || distance < 0 || Double.isNaN(distance) || Double.isFinite(distance)) { + swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(0,0,0), false); return; } double desiredAlignmentAngle = getDesiredAlignmentAngle(tagId); - double driveSpeed = distanceController.calculate(targetDistance, distance); + double driveSpeed = distanceController.calculate(distance, TARGET_DISTANCE_METERS); double rotationSpeed = rotationController.calculate(offset, desiredAlignmentAngle); + if (distanceController.atSetpoint()){ + driveSpeed = 0.0; + } + if (rotationController.atSetpoint()){ + rotationSpeed = 0.0; + } + + driveSpeed = MathUtil.clamp(driveSpeed, -MAX_FORWARD_SPEED_MPS, MAX_FORWARD_SPEED_MPS); + rotationSpeed = MathUtil.clamp(rotationSpeed, -MAX_ROTATION_SPEED_RAD_PER_SEC, MAX_ROTATION_SPEED_RAD_PER_SEC); + swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(driveSpeed, 0, rotationSpeed), false); } From bee0fc909cbc35ab14842a4550f4298c81c5c8ec Mon Sep 17 00:00:00 2001 From: Seqi Date: Mon, 20 Apr 2026 15:22:07 -0500 Subject: [PATCH 58/96] pid --- src/main/java/frc/robot/Command/SimpleAutoAlign.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Command/SimpleAutoAlign.java b/src/main/java/frc/robot/Command/SimpleAutoAlign.java index 1553a1e..f400314 100644 --- a/src/main/java/frc/robot/Command/SimpleAutoAlign.java +++ b/src/main/java/frc/robot/Command/SimpleAutoAlign.java @@ -20,12 +20,12 @@ public class SimpleAutoAlign extends Command { public static final double CAMERA_TILT_DEG = 0.000001; //Distance PID tuning - public static final double DISTANCE_KP = 0; + public static final double DISTANCE_KP = 0.05; public static final double DISTANCE_KI = 0; public static final double DISTANCE_KD = 0; //Rotation PID tuning - public static final double ROTATION_KP = 0; + public static final double ROTATION_KP = 0.02; public static final double ROTATION_KI = 0; public static final double ROTATION_KD = 0; @@ -37,9 +37,9 @@ public class SimpleAutoAlign extends Command { private final PIDController distanceController = new PIDController(DISTANCE_KP, DISTANCE_KI, DISTANCE_KD); private final PIDController rotationController = new PIDController(ROTATION_KP, ROTATION_KI, ROTATION_KD); - + public SimpleAutoAlign(SwerveSubsystem swerveSubsystem){ this.swerveSubsystem = swerveSubsystem; addRequirements(swerveSubsystem); From f058ba9aae280cb2e52cc9095d3895357801e042 Mon Sep 17 00:00:00 2001 From: Seqi Date: Mon, 20 Apr 2026 16:11:58 -0500 Subject: [PATCH 59/96] fixed bug with auto align (simple) --- src/main/java/frc/robot/Command/SimpleAutoAlign.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Command/SimpleAutoAlign.java b/src/main/java/frc/robot/Command/SimpleAutoAlign.java index f400314..722b077 100644 --- a/src/main/java/frc/robot/Command/SimpleAutoAlign.java +++ b/src/main/java/frc/robot/Command/SimpleAutoAlign.java @@ -17,7 +17,7 @@ public class SimpleAutoAlign extends Command { //Camera geometry public static final double CAMERA_HEIGHT_METERS = 0.5; public static final double APRIL_TAG_HEIGHT_METERS = 1.0; - public static final double CAMERA_TILT_DEG = 0.000001; + public static final double CAMERA_TILT_DEG = 0.0; //Distance PID tuning public static final double DISTANCE_KP = 0.05; @@ -39,7 +39,7 @@ public class SimpleAutoAlign extends Command { private final PIDController rotationController = new PIDController(ROTATION_KP, ROTATION_KI, ROTATION_KD); - + public SimpleAutoAlign(SwerveSubsystem swerveSubsystem){ this.swerveSubsystem = swerveSubsystem; addRequirements(swerveSubsystem); @@ -93,7 +93,7 @@ public void execute() { int tagId = getTagId(); //only auto align if distance is valid and can see tag 10 - if (!canSeeTag() || !isSupportedTag(tagId) || distance < 0 || Double.isNaN(distance) || Double.isFinite(distance)) { + if (!canSeeTag() || !isSupportedTag(tagId) || distance < 0 || !Double.isFinite(distance)) { swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(0,0,0), false); return; From ab05e6ade5d725a7ab3433c6dd37ad422c2a6cdc Mon Sep 17 00:00:00 2001 From: Seqi Date: Fri, 24 Apr 2026 17:40:50 -0500 Subject: [PATCH 60/96] new auto align lock --- .../frc/robot/Command/SimpleAutoAlign.java | 66 +++++++++++++++---- 1 file changed, 54 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Command/SimpleAutoAlign.java b/src/main/java/frc/robot/Command/SimpleAutoAlign.java index 722b077..a561276 100644 --- a/src/main/java/frc/robot/Command/SimpleAutoAlign.java +++ b/src/main/java/frc/robot/Command/SimpleAutoAlign.java @@ -34,9 +34,15 @@ public class SimpleAutoAlign extends Command { public static final double ROTATION_TOLERANCE_DEG = 1.5; public static final double MAX_FORWARD_SPEED_MPS = 1.25; public static final double MAX_ROTATION_SPEED_RAD_PER_SEC = 2.5; + public static final double MIN_DISTANCE_CALC_ANGLE_DEG = 1.0; + public static final int SETTLE_CYCLES_REQUIRED = 10; + public static final double UNLOCK_DISTANCE_ERROR_METERS = 0.15; + public static final double UNLOCK_ROTATION_ERROR_DEG = 3.0; private final PIDController distanceController = new PIDController(DISTANCE_KP, DISTANCE_KI, DISTANCE_KD); private final PIDController rotationController = new PIDController(ROTATION_KP, ROTATION_KI, ROTATION_KD); + private int settledCycles = 0; + private boolean alignmentLocked = false; @@ -68,16 +74,30 @@ private double getDesiredAlignmentAngle(int tagId) { return 0.0; } - private double getDistanceToTarget() { - double ty = NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("ty").getDouble(0.0); // vertical angle offset in degrees - double angleToTargetRadians = Math.toRadians(CAMERA_TILT_DEG + ty); - return (APRIL_TAG_HEIGHT_METERS - CAMERA_HEIGHT_METERS) / Math.tan(angleToTargetRadians); + private double getVerticalOffsetToTarget(){ + return NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("ty").getDouble(0.0); } - private double getOffsetToTarget() { return NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("tx").getDouble(0.0); } + private double getDistanceToTargetMeters(double tyDegrees){ + double angleToTargetDegrees = CAMERA_TILT_DEG + tyDegrees; + double tangent = Math.tan(angleToTargetDegrees); + if (Math.abs(angleToTargetDegrees) UNLOCK_ROTATION_ERROR_DEG || Math.abs(distanceError) > UNLOCK_DISTANCE_ERROR_METERS){ + alignmentLocked = false; + } + + if (settledCycles >= SETTLE_CYCLES_REQUIRED){ + alignmentLocked = true; } - if (rotationController.atSetpoint()){ + + if (alignmentLocked){ + swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(0, 0, 0), false); + return; + } + double driveSpeed = 0.0; + + if (withinRotationTolerance){ rotationSpeed = 0.0; + } else if (Math.abs(rotationError)<(ROTATION_TOLERANCE_DEG*2.0)){ + rotationSpeed*=0.5; } driveSpeed = MathUtil.clamp(driveSpeed, -MAX_FORWARD_SPEED_MPS, MAX_FORWARD_SPEED_MPS); From 7d60fc0e586cd422ed50ccf4be5761aa9204c799 Mon Sep 17 00:00:00 2001 From: Seqi Date: Sat, 25 Apr 2026 12:39:08 -0500 Subject: [PATCH 61/96] Added hopper motor --- src/main/java/frc/robot/Constants.java | 2 ++ src/main/java/frc/robot/RobotContainer.java | 6 +++++- .../java/frc/robot/Subsystems/ShooterSubsystem.java | 12 ++++++++++++ 3 files changed, 19 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 24fcb54..36e13c0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -283,11 +283,13 @@ public static final class ShooterConstants { public static final int KICKER_ID = 21; public static final int HOOD_ID = 20; public static final int INDEXER_ID = 23; + public static final int HOPPER_ID = 100; //placeholder // Percent output caps ([-1..1]). Higher = faster spin-up but more current draw. public static final double SHOOTER_SPEED = 0.6; public static final double KICKER_SPEED = 0.6; public static final double INDEXER_SPEED = 0.4; //placeholder + public static final double HOPPER_SPEED = 0.5; //placeholder // Shooter readiness (SparkMax encoder velocity is RPM). Tune on the real robot. public static final double SHOOTER_READY_RPM = 3000.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 59df386..15755c5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -162,23 +162,27 @@ private void configureBindings() { double kickerSpeed = 0.0; double indexerSpeed = 0.0; + double hopperSpeed = 0.0; if (leftBumperPressed) { kickerSpeed = -ShooterConstants.KICKER_SPEED; indexerSpeed = -ShooterConstants.INDEXER_SPEED; + hopperSpeed = -ShooterConstants.HOPPER_SPEED; } else if (rightBumperPressed) { kickerSpeed = ShooterConstants.KICKER_SPEED; indexerSpeed = indexerEnabled ? ShooterConstants.INDEXER_SPEED : 0.0; + hopperSpeed = indexerEnabled ? ShooterConstants.HOPPER_SPEED : 0.0; } m_shooter.setKickerSpeed(kickerSpeed); m_shooter.setIndexerSpeed(indexerSpeed); + m_shooter.setHopperSpeed(hopperSpeed); lastHelmsRightBumperPressed = rightBumperPressed; }, m_shooter)); - + // Hood controls (helms controller). // Y = hood up (2 inches / max travel), B = hood down. helmsController.y().onTrue(new InstantCommand(() -> m_shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), m_shooter)); diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index 92829fa..e5dde3f 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -23,10 +23,12 @@ public class ShooterSubsystem extends SubsystemBase { private final SparkMax kickerMotor = new SparkMax(ShooterConstants.KICKER_ID, MotorType.kBrushless); private final SparkMax hoodMotor = new SparkMax(ShooterConstants.HOOD_ID, MotorType.kBrushless); private final SparkMax indexerMotor = new SparkMax(ShooterConstants.INDEXER_ID, MotorType.kBrushless); + private final SparkMax hopperMotor = new SparkMax(ShooterConstants.HOPPER_ID, MotorType.kBrushless); private double shooterCmd = 0.0; private double kickerCmd = 0.0; private double indexerCmd = 0.0; + private double hopperCmd = 0.0; private final PIDController hoodController = new PIDController( ShooterConstants.HOOD_KP, @@ -65,12 +67,17 @@ public ShooterSubsystem() { indexConfig.inverted(true); indexConfig.idleMode(IdleMode.kBrake); + SparkMaxConfig hopperConfig = new SparkMaxConfig(); + hopperConfig.inverted(false); //need to find out + hopperConfig.idleMode(IdleMode.kBrake); + shooterMotor.configure(shootConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); kickerMotor.configure(feedConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); hoodMotor.configure(hoodConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); indexerMotor.configure(indexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + hopperMotor.configure(hopperConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); hoodController.setTolerance(ShooterConstants.HOOD_TOLERANCE); } @@ -117,6 +124,11 @@ public void setKickerSpeed(double speed) { kickerMotor.set(speed); } + public void setHopperSpeed(double speed) { + hopperCmd = speed; + hopperMotor.set(speed); + } + public void setHoodAngle(HoodAngle angle) { switch (angle) { case LOW: From e3f3b79ebe783e43062060055ab72dbbe388a013 Mon Sep 17 00:00:00 2001 From: Seqi Date: Mon, 27 Apr 2026 15:08:14 -0500 Subject: [PATCH 62/96] updated motor id --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 36e13c0..5719a69 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -283,7 +283,7 @@ public static final class ShooterConstants { public static final int KICKER_ID = 21; public static final int HOOD_ID = 20; public static final int INDEXER_ID = 23; - public static final int HOPPER_ID = 100; //placeholder + public static final int HOPPER_ID = 43; //placeholder // Percent output caps ([-1..1]). Higher = faster spin-up but more current draw. public static final double SHOOTER_SPEED = 0.6; From c5909c2d8ff6e92b1dfd158cbc3973ca8a7841c6 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Thu, 30 Apr 2026 10:27:08 -0500 Subject: [PATCH 63/96] Updated Phoenix to 2026 2.0 version --- ...c2026-latest.json => Phoenix6-26.2.0.json} | 62 +++++++++---------- vendordeps/REVLib.json | 18 +++--- 2 files changed, 40 insertions(+), 40 deletions(-) rename vendordeps/{Phoenix6-frc2026-latest.json => Phoenix6-26.2.0.json} (92%) diff --git a/vendordeps/Phoenix6-frc2026-latest.json b/vendordeps/Phoenix6-26.2.0.json similarity index 92% rename from vendordeps/Phoenix6-frc2026-latest.json rename to vendordeps/Phoenix6-26.2.0.json index 8f6e30f..e4bde96 100644 --- a/vendordeps/Phoenix6-frc2026-latest.json +++ b/vendordeps/Phoenix6-26.2.0.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-frc2026-latest.json", + "fileName": "Phoenix6-26.2.0.json", "name": "CTRE-Phoenix (v6)", - "version": "26.1.0", + "version": "26.2.0", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.1.0" + "version": "26.2.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.1.0", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.1.0", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.1.0", + "version": "26.2.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.1.0", + "version": "26.2.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.1.0", + "version": "26.2.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.1.0", + "version": "26.2.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.1.0", + "version": "26.2.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.1.0", + "version": "26.2.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.1.0", + "version": "26.2.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.1.0", + "version": "26.2.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.1.0", + "version": "26.2.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.1.0", + "version": "26.2.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.1.0", + "version": "26.2.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.1.0", + "version": "26.2.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.1.0", + "version": "26.2.0", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.1.0", + "version": "26.2.0", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index d35e593..c6b1b9e 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2026.0.1", + "version": "2026.0.5", "frcYear": "2026", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2026.0.1" + "version": "2026.0.5" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2026.0.5", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2026.0.5", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -49,7 +49,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2026.0.5", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -66,7 +66,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2026.0.1", + "version": "2026.0.5", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -83,7 +83,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.1", + "version": "2026.0.5", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -100,7 +100,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", + "version": "2026.0.5", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -116,7 +116,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", + "version": "2026.0.5", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, From 33fd9c65604eda08894b0fc6f9f15f2dbf77b314 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Thu, 30 Apr 2026 10:28:14 -0500 Subject: [PATCH 64/96] Change speed for Panther Party REMEMBER TO CHANGE IT BACK --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 15755c5..d52f59e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -200,11 +200,11 @@ private void configureBindings() { // SwerveSubsystem - The drive subsystem to control m_drive, // translationSupplier - Forward/backward speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 1.0, + () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.3, // strafeSupplier - Side-to-side speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 1.0, + () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.3, // rotationSupplier - Rotation speed - () -> -driveController.getRawAxis(rotationAxis) * 0.5, + () -> -driveController.getRawAxis(rotationAxis) * 0.2, // robotCentricSupplier - Robot-oriented (true) vs field-oriented (false) () -> robotCentric.getAsBoolean(), // isAutoAlignSupplier - Auto-align active flag From 2c260cc6fd743f0ab1a4d97811bb0efbd461d9f8 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Fri, 1 May 2026 15:44:41 -0500 Subject: [PATCH 65/96] Revert "Change speed for Panther Party" This reverts commit 33fd9c65604eda08894b0fc6f9f15f2dbf77b314. --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d52f59e..15755c5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -200,11 +200,11 @@ private void configureBindings() { // SwerveSubsystem - The drive subsystem to control m_drive, // translationSupplier - Forward/backward speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.3, + () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 1.0, // strafeSupplier - Side-to-side speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.3, + () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 1.0, // rotationSupplier - Rotation speed - () -> -driveController.getRawAxis(rotationAxis) * 0.2, + () -> -driveController.getRawAxis(rotationAxis) * 0.5, // robotCentricSupplier - Robot-oriented (true) vs field-oriented (false) () -> robotCentric.getAsBoolean(), // isAutoAlignSupplier - Auto-align active flag From c03e891adf3b97ea8fcca38272da7d8d6badca43 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Fri, 1 May 2026 15:44:47 -0500 Subject: [PATCH 66/96] Revert "Updated Phoenix to 2026 2.0 version" This reverts commit c5909c2d8ff6e92b1dfd158cbc3973ca8a7841c6. --- ....2.0.json => Phoenix6-frc2026-latest.json} | 62 +++++++++---------- vendordeps/REVLib.json | 18 +++--- 2 files changed, 40 insertions(+), 40 deletions(-) rename vendordeps/{Phoenix6-26.2.0.json => Phoenix6-frc2026-latest.json} (92%) diff --git a/vendordeps/Phoenix6-26.2.0.json b/vendordeps/Phoenix6-frc2026-latest.json similarity index 92% rename from vendordeps/Phoenix6-26.2.0.json rename to vendordeps/Phoenix6-frc2026-latest.json index e4bde96..8f6e30f 100644 --- a/vendordeps/Phoenix6-26.2.0.json +++ b/vendordeps/Phoenix6-frc2026-latest.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-26.2.0.json", + "fileName": "Phoenix6-frc2026-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "26.2.0", + "version": "26.1.0", "frcYear": "2026", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.2.0" + "version": "26.1.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.2.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.2.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.2.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.2.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.2.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.2.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.2.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.2.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.2.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.2.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.2.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.2.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.2.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -208,7 +208,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.2.0", + "version": "26.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -224,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.2.0", + "version": "26.1.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -240,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.2.0", + "version": "26.1.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.2.0", + "version": "26.1.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.2.0", + "version": "26.1.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.2.0", + "version": "26.1.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.2.0", + "version": "26.1.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.2.0", + "version": "26.1.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.2.0", + "version": "26.1.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.2.0", + "version": "26.1.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.2.0", + "version": "26.1.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.2.0", + "version": "26.1.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.2.0", + "version": "26.1.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.2.0", + "version": "26.1.0", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.2.0", + "version": "26.1.0", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index c6b1b9e..d35e593 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2026.0.5", + "version": "2026.0.1", "frcYear": "2026", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2026.0.5" + "version": "2026.0.1" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.5", + "version": "2026.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.5", + "version": "2026.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -49,7 +49,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.5", + "version": "2026.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -66,7 +66,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2026.0.5", + "version": "2026.0.1", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -83,7 +83,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.5", + "version": "2026.0.1", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -100,7 +100,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.5", + "version": "2026.0.1", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -116,7 +116,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.5", + "version": "2026.0.1", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, From b5b3015d313ae24676d0f28d4144aa9c77f3a2f6 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Mon, 4 May 2026 16:26:42 -0500 Subject: [PATCH 67/96] reverse motor --- src/main/java/frc/robot/Subsystems/ShooterSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java index e5dde3f..ffe678f 100644 --- a/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/ShooterSubsystem.java @@ -68,8 +68,8 @@ public ShooterSubsystem() { indexConfig.idleMode(IdleMode.kBrake); SparkMaxConfig hopperConfig = new SparkMaxConfig(); - hopperConfig.inverted(false); //need to find out - hopperConfig.idleMode(IdleMode.kBrake); + hopperConfig.inverted(true); + hopperConfig.idleMode(IdleMode.kCoast); From a79fd18943f50421617f3f3060e1e0c6cf9858dd Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Mon, 4 May 2026 17:38:41 -0500 Subject: [PATCH 68/96] target distance --- src/main/java/frc/robot/Command/SimpleAutoAlign.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Command/SimpleAutoAlign.java b/src/main/java/frc/robot/Command/SimpleAutoAlign.java index a561276..aaf91e3 100644 --- a/src/main/java/frc/robot/Command/SimpleAutoAlign.java +++ b/src/main/java/frc/robot/Command/SimpleAutoAlign.java @@ -38,6 +38,7 @@ public class SimpleAutoAlign extends Command { public static final int SETTLE_CYCLES_REQUIRED = 10; public static final double UNLOCK_DISTANCE_ERROR_METERS = 0.15; public static final double UNLOCK_ROTATION_ERROR_DEG = 3.0; + public static final double DESIRED_DISTANCE_FROM_APRILTAG = 2.0; private final PIDController distanceController = new PIDController(DISTANCE_KP, DISTANCE_KI, DISTANCE_KD); private final PIDController rotationController = new PIDController(ROTATION_KP, ROTATION_KI, ROTATION_KD); @@ -84,6 +85,7 @@ private double getOffsetToTarget() { private double getDistanceToTargetMeters(double tyDegrees){ double angleToTargetDegrees = CAMERA_TILT_DEG + tyDegrees; double tangent = Math.tan(angleToTargetDegrees); + double target = TARGET_DISTANCE_METERS; if (Math.abs(angleToTargetDegrees) Date: Mon, 4 May 2026 18:05:29 -0500 Subject: [PATCH 69/96] speed back to demo --- .../java/frc/robot/Command/SimpleAutoAlign.java | 14 ++++++++++---- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 6 +++--- 3 files changed, 14 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Command/SimpleAutoAlign.java b/src/main/java/frc/robot/Command/SimpleAutoAlign.java index aaf91e3..3b03c42 100644 --- a/src/main/java/frc/robot/Command/SimpleAutoAlign.java +++ b/src/main/java/frc/robot/Command/SimpleAutoAlign.java @@ -20,7 +20,7 @@ public class SimpleAutoAlign extends Command { public static final double CAMERA_TILT_DEG = 0.0; //Distance PID tuning - public static final double DISTANCE_KP = 0.05; + public static final double DISTANCE_KP = 0.02; public static final double DISTANCE_KI = 0; public static final double DISTANCE_KD = 0; @@ -38,7 +38,6 @@ public class SimpleAutoAlign extends Command { public static final int SETTLE_CYCLES_REQUIRED = 10; public static final double UNLOCK_DISTANCE_ERROR_METERS = 0.15; public static final double UNLOCK_ROTATION_ERROR_DEG = 3.0; - public static final double DESIRED_DISTANCE_FROM_APRILTAG = 2.0; private final PIDController distanceController = new PIDController(DISTANCE_KP, DISTANCE_KI, DISTANCE_KD); private final PIDController rotationController = new PIDController(ROTATION_KP, ROTATION_KI, ROTATION_KD); @@ -85,7 +84,6 @@ private double getOffsetToTarget() { private double getDistanceToTargetMeters(double tyDegrees){ double angleToTargetDegrees = CAMERA_TILT_DEG + tyDegrees; double tangent = Math.tan(angleToTargetDegrees); - double target = TARGET_DISTANCE_METERS; if (Math.abs(angleToTargetDegrees) -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 1.0, + () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.3, // strafeSupplier - Side-to-side speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 1.0, + () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.3, // rotationSupplier - Rotation speed - () -> -driveController.getRawAxis(rotationAxis) * 0.5, + () -> -driveController.getRawAxis(rotationAxis) * 0.2, // robotCentricSupplier - Robot-oriented (true) vs field-oriented (false) () -> robotCentric.getAsBoolean(), // isAutoAlignSupplier - Auto-align active flag From 2715f142691e84639469d75770715a81cc0cbc9f Mon Sep 17 00:00:00 2001 From: Seqi Date: Mon, 4 May 2026 18:15:39 -0500 Subject: [PATCH 70/96] fixed typo --- src/main/java/frc/robot/Command/SimpleAutoAlign.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Command/SimpleAutoAlign.java b/src/main/java/frc/robot/Command/SimpleAutoAlign.java index 3b03c42..5e9104d 100644 --- a/src/main/java/frc/robot/Command/SimpleAutoAlign.java +++ b/src/main/java/frc/robot/Command/SimpleAutoAlign.java @@ -162,7 +162,7 @@ public void execute() { swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(0, driveSpeed, rotationSpeed), false); System.out.println("settled cycles: " + settledCycles); -ri + } @Override From 8bee376d90e453667adc37841a492fc1ebbd244d Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Fri, 8 May 2026 15:32:41 -0500 Subject: [PATCH 71/96] simple auto align --- .../frc/robot/Command/SimpleAutoAlign.java | 70 +++++++++++-------- 1 file changed, 42 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/Command/SimpleAutoAlign.java b/src/main/java/frc/robot/Command/SimpleAutoAlign.java index 5e9104d..818b51f 100644 --- a/src/main/java/frc/robot/Command/SimpleAutoAlign.java +++ b/src/main/java/frc/robot/Command/SimpleAutoAlign.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Command; @@ -20,7 +21,7 @@ public class SimpleAutoAlign extends Command { public static final double CAMERA_TILT_DEG = 0.0; //Distance PID tuning - public static final double DISTANCE_KP = 0.02; + public static final double DISTANCE_KP = 0.6; public static final double DISTANCE_KI = 0; public static final double DISTANCE_KD = 0; @@ -32,17 +33,21 @@ public class SimpleAutoAlign extends Command { //Tolerance and public static final double DISTANCE_TOLERANCE_METERS = 0.08; public static final double ROTATION_TOLERANCE_DEG = 1.5; - public static final double MAX_FORWARD_SPEED_MPS = 1.25; + public static final double MAX_FORWARD_SPEED_MPS = 0.75; public static final double MAX_ROTATION_SPEED_RAD_PER_SEC = 2.5; public static final double MIN_DISTANCE_CALC_ANGLE_DEG = 1.0; public static final int SETTLE_CYCLES_REQUIRED = 10; public static final double UNLOCK_DISTANCE_ERROR_METERS = 0.15; public static final double UNLOCK_ROTATION_ERROR_DEG = 3.0; + public static final double DISTANCE_FILTER_ALPHA = 0.25; + public static final double MAX_DISTANCE_ACCEL_MPS_PER_SEC = 1.5; private final PIDController distanceController = new PIDController(DISTANCE_KP, DISTANCE_KI, DISTANCE_KD); private final PIDController rotationController = new PIDController(ROTATION_KP, ROTATION_KI, ROTATION_KD); private int settledCycles = 0; private boolean alignmentLocked = false; + private double filteredDistanceMeters = Double.NaN; + private final SlewRateLimiter distanceSpeedLimiter = new SlewRateLimiter(MAX_DISTANCE_ACCEL_MPS_PER_SEC); @@ -83,11 +88,10 @@ private double getOffsetToTarget() { private double getDistanceToTargetMeters(double tyDegrees){ double angleToTargetDegrees = CAMERA_TILT_DEG + tyDegrees; - double tangent = Math.tan(angleToTargetDegrees); if (Math.abs(angleToTargetDegrees) UNLOCK_ROTATION_ERROR_DEG || Math.abs(distanceError) > UNLOCK_DISTANCE_ERROR_METERS){ - alignmentLocked = false; + if (withinRotationTolerance && withinDistanceTolerance){ + settledCycles++; + } else { + settledCycles = 0; } - if (settledCycles >= SETTLE_CYCLES_REQUIRED){ - alignmentLocked = true; - } - + alignmentLocked = settledCycles >= SETTLE_CYCLES_REQUIRED; if (alignmentLocked){ + distanceSpeedLimiter.reset(0.0); swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(0, 0, 0), false); return; } - double driveSpeed = 0.5; - - if (withinRotationTolerance){ - rotationSpeed = 0.0; - } else if (Math.abs(rotationError)<(ROTATION_TOLERANCE_DEG*2.0)){ - rotationSpeed*=0.5; - } + double driveSpeed = 0.0; - if (withinDistanceTolerance) { - driveSpeed = 0; + if (distanceIsValid && Math.abs (rotationError) <= UNLOCK_ROTATION_ERROR_DEG &&! withinDistanceTolerance){ + driveSpeed = distanceController.calculate(0.0, distanceError); } + driveSpeed = MathUtil.clamp(driveSpeed, -MAX_FORWARD_SPEED_MPS, MAX_FORWARD_SPEED_MPS); + driveSpeed = distanceSpeedLimiter.calculate(driveSpeed); rotationSpeed = MathUtil.clamp(rotationSpeed, -MAX_ROTATION_SPEED_RAD_PER_SEC, MAX_ROTATION_SPEED_RAD_PER_SEC); swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(0, driveSpeed, rotationSpeed), false); - System.out.println("settled cycles: " + settledCycles); - } @Override From 2d1fd012a0fa34570e02ab7b2f48d52134a2000c Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Fri, 8 May 2026 15:34:17 -0500 Subject: [PATCH 72/96] changed the speed back up --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2c2ba32..5719a69 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -286,7 +286,7 @@ public static final class ShooterConstants { public static final int HOPPER_ID = 43; //placeholder // Percent output caps ([-1..1]). Higher = faster spin-up but more current draw. - public static final double SHOOTER_SPEED = 0.3; + public static final double SHOOTER_SPEED = 0.6; public static final double KICKER_SPEED = 0.6; public static final double INDEXER_SPEED = 0.4; //placeholder public static final double HOPPER_SPEED = 0.5; //placeholder diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d52f59e..15755c5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -200,11 +200,11 @@ private void configureBindings() { // SwerveSubsystem - The drive subsystem to control m_drive, // translationSupplier - Forward/backward speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 0.3, + () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 1.0, // strafeSupplier - Side-to-side speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 0.3, + () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 1.0, // rotationSupplier - Rotation speed - () -> -driveController.getRawAxis(rotationAxis) * 0.2, + () -> -driveController.getRawAxis(rotationAxis) * 0.5, // robotCentricSupplier - Robot-oriented (true) vs field-oriented (false) () -> robotCentric.getAsBoolean(), // isAutoAlignSupplier - Auto-align active flag From 92cdac58e66c383343e264adfc38aad1a8a5afae Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Fri, 8 May 2026 16:10:16 -0500 Subject: [PATCH 73/96] speeded up auto align --- src/main/java/frc/robot/Command/SimpleAutoAlign.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Command/SimpleAutoAlign.java b/src/main/java/frc/robot/Command/SimpleAutoAlign.java index 818b51f..94eb07a 100644 --- a/src/main/java/frc/robot/Command/SimpleAutoAlign.java +++ b/src/main/java/frc/robot/Command/SimpleAutoAlign.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.SwerveConstants; import frc.robot.Subsystems.SwerveSubsystem; public class SimpleAutoAlign extends Command { @@ -21,7 +22,7 @@ public class SimpleAutoAlign extends Command { public static final double CAMERA_TILT_DEG = 0.0; //Distance PID tuning - public static final double DISTANCE_KP = 0.6; + public static final double DISTANCE_KP = 2.5; public static final double DISTANCE_KI = 0; public static final double DISTANCE_KD = 0; @@ -33,14 +34,14 @@ public class SimpleAutoAlign extends Command { //Tolerance and public static final double DISTANCE_TOLERANCE_METERS = 0.08; public static final double ROTATION_TOLERANCE_DEG = 1.5; - public static final double MAX_FORWARD_SPEED_MPS = 0.75; + public static final double MAX_FORWARD_SPEED_MPS = SwerveConstants.maxSpeed; public static final double MAX_ROTATION_SPEED_RAD_PER_SEC = 2.5; public static final double MIN_DISTANCE_CALC_ANGLE_DEG = 1.0; public static final int SETTLE_CYCLES_REQUIRED = 10; public static final double UNLOCK_DISTANCE_ERROR_METERS = 0.15; public static final double UNLOCK_ROTATION_ERROR_DEG = 3.0; public static final double DISTANCE_FILTER_ALPHA = 0.25; - public static final double MAX_DISTANCE_ACCEL_MPS_PER_SEC = 1.5; + public static final double MAX_DISTANCE_ACCEL_MPS_PER_SEC = SwerveConstants.maxSpeed * 4.0; private final PIDController distanceController = new PIDController(DISTANCE_KP, DISTANCE_KI, DISTANCE_KD); private final PIDController rotationController = new PIDController(ROTATION_KP, ROTATION_KI, ROTATION_KD); From 976eed28e8b9564159c37d5c1ad36dbab7c3203b Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Mon, 11 May 2026 16:44:34 -0500 Subject: [PATCH 74/96] added hopper motor to autos and a new left neutral zone auto --- .../java/frc/robot/Auto/CenterLemonAuto.java | 1 + .../frc/robot/Auto/CenterToDepotAuto.java | 1 + .../frc/robot/Auto/DepotShootingAuto.java | 1 + .../java/frc/robot/Auto/LeftLemonAuto.java | 1 + .../frc/robot/Auto/LeftNeutralZoneAuto1.java | 1 + .../frc/robot/Auto/LeftNeutralZoneAuto2.java | 197 ++++++++++++++++++ .../java/frc/robot/Auto/RightLemonAuto.java | 1 + .../frc/robot/Auto/RightNeutralZoneAuto1.java | 1 + src/main/java/frc/robot/RobotContainer.java | 4 +- .../{Auto => UnusedAuto}/DriveTestAuto.java | 2 +- .../{Auto => UnusedAuto}/ShootEightAuto.java | 3 +- 11 files changed, 209 insertions(+), 4 deletions(-) create mode 100644 src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java rename src/main/java/frc/robot/{Auto => UnusedAuto}/DriveTestAuto.java (95%) rename src/main/java/frc/robot/{Auto => UnusedAuto}/ShootEightAuto.java (94%) diff --git a/src/main/java/frc/robot/Auto/CenterLemonAuto.java b/src/main/java/frc/robot/Auto/CenterLemonAuto.java index 7fe6d5f..028cc56 100644 --- a/src/main/java/frc/robot/Auto/CenterLemonAuto.java +++ b/src/main/java/frc/robot/Auto/CenterLemonAuto.java @@ -40,6 +40,7 @@ public CenterLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSub shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ) ); diff --git a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java index d54d60f..7724d4e 100644 --- a/src/main/java/frc/robot/Auto/CenterToDepotAuto.java +++ b/src/main/java/frc/robot/Auto/CenterToDepotAuto.java @@ -93,6 +93,7 @@ public CenterToDepotAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS Commands.run(() -> { shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ), diff --git a/src/main/java/frc/robot/Auto/DepotShootingAuto.java b/src/main/java/frc/robot/Auto/DepotShootingAuto.java index 707e044..83205cb 100644 --- a/src/main/java/frc/robot/Auto/DepotShootingAuto.java +++ b/src/main/java/frc/robot/Auto/DepotShootingAuto.java @@ -91,6 +91,7 @@ public DepotShootingAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterS Commands.run(() -> { shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ), diff --git a/src/main/java/frc/robot/Auto/LeftLemonAuto.java b/src/main/java/frc/robot/Auto/LeftLemonAuto.java index a23d4e4..6c2f328 100644 --- a/src/main/java/frc/robot/Auto/LeftLemonAuto.java +++ b/src/main/java/frc/robot/Auto/LeftLemonAuto.java @@ -54,6 +54,7 @@ public LeftLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsy shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ) ); diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index f9eb1a7..8d99d78 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -91,6 +91,7 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot Commands.run(() -> { shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ), diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java new file mode 100644 index 0000000..83f528b --- /dev/null +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java @@ -0,0 +1,197 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; + +import java.util.concurrent.atomic.AtomicReference; + + +public class LeftNeutralZoneAuto2 extends SequentialCommandGroup { + private static final double DRIVE_SPEED_MPS = 2.0; + private static final double DRIVE_HEADING_P = 3.0; + private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; + private static final double TURN_P = 4.0; + private static final double TURN_TOLERANCE_DEG = 3.0; + private static final double TURN_TIMEOUT_SEC = 2.5; + + private static final double BACKWARD_METERS_1 = 3.6; + private static final double BACKWARD_METERS_2 = 4.3; + private static final double FORWARD_METERS_1 = 3.0; + //private static final double FORWARD_METERS_2 = 1.0; + //private static final double FORWARD_METERS_3 = 3.2; + + private static final double INTAKE_POWER = -0.75; + + public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + addRequirements(drive, intake, shooter); + + addCommands( + Commands.runOnce(intake::lowerIntake, intake), + + // Drive backwards 3.6m. + driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS), + + // Turn 90 degrees left. + turnRelativeDegrees(drive, 90.0), + + // Drive forward 3m while starting intake (intake stays on for the rest of auto). + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Drive backwards 3m (intake still on). + driveDistanceMeters(drive, -FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Stop intake at the end. + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 90 degrees left + turnRelativeDegrees(drive, 90.0), + + // Drive backward (back to the trench) + driveDistanceMeters(drive, -BACKWARD_METERS_2, DRIVE_SPEED_MPS), + + // Turn 20 degrees left + turnRelativeDegrees(drive, 20.0), + + // Bring hood up to HIGH angle. + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + + // Shooter + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(1.0), + + // Keep intake running while the intake arm cycles up/down during shooting. + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + + Commands.parallel( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + }, shooter) + ), + + Commands.waitSeconds(1), + + // While shooting/indexing, continuously move the intake arm up/down. + Commands.sequence( + Commands.runOnce(intake::lowerIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), + Commands.runOnce(intake::raiseIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) + ) + .repeatedly() + ) + ); + } + + private static edu.wpi.first.wpilibj2.command.Command driveDistanceMeters( + SwerveSubsystem drive, + double distanceMeters, + double speedMps) { + double clampedSpeedMps = MathUtil.clamp(Math.abs(speedMps), 0.0, SwerveConstants.maxSpeed); + double commandedSpeedMps = Math.copySign(clampedSpeedMps, distanceMeters); + double distanceAbsMeters = Math.abs(distanceMeters); + + AtomicReference startPositions = new AtomicReference<>(); + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> { + startPositions.set(drive.getPositions()); + startYawRad[0] = drive.getYaw().getRadians(); + }, drive), + Commands.runEnd( + () -> { + double errorRad = MathUtil.angleModulus(startYawRad[0] - drive.getYaw().getRadians()); + double maxOmegaRadPerSec = + Math.min(DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC, SwerveConstants.maxAngularVelocity); + double omegaRadPerSec = + MathUtil.clamp(errorRad * DRIVE_HEADING_P, -maxOmegaRadPerSec, maxOmegaRadPerSec); + drive.drive(commandedSpeedMps, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until( + () -> getAverageWheelDeltaMeters(startPositions.get(), drive.getPositions()) >= distanceAbsMeters) + .withTimeout(distanceAbsMeters / Math.max(0.1, Math.abs(commandedSpeedMps)) + 1.0) + ); + } + + private static double getAverageWheelDeltaMeters( + SwerveModulePosition[] startPositions, + SwerveModulePosition[] currentPositions) { + if (startPositions == null || currentPositions == null) { + return 0.0; + } + + int count = Math.min(startPositions.length, currentPositions.length); + if (count <= 0) { + return 0.0; + } + + double sum = 0.0; + for (int i = 0; i < count; i++) { + sum += Math.abs(currentPositions[i].distanceMeters - startPositions[i].distanceMeters); + } + return sum / count; + } + + private static edu.wpi.first.wpilibj2.command.Command turnRelativeDegrees( + SwerveSubsystem drive, + double deltaDegrees) { + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.runEnd( + () -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadPerSec = + MathUtil.clamp( + errorRad * TURN_P, + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + drive.drive(0, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(TURN_TOLERANCE_DEG); + }) + .withTimeout(TURN_TIMEOUT_SEC) + + ); + } +} diff --git a/src/main/java/frc/robot/Auto/RightLemonAuto.java b/src/main/java/frc/robot/Auto/RightLemonAuto.java index d361b89..800cd31 100644 --- a/src/main/java/frc/robot/Auto/RightLemonAuto.java +++ b/src/main/java/frc/robot/Auto/RightLemonAuto.java @@ -54,6 +54,7 @@ public RightLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubs shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ) ); diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java index e17e19f..7583483 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java @@ -89,6 +89,7 @@ public RightNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoo Commands.run(() -> { shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ), diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 15755c5..8f68a37 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,11 +24,9 @@ import frc.robot.Auto.LeftNeutralZoneAuto1; import frc.robot.Auto.RightNeutralZoneAuto1; import frc.robot.Auto.RightLemonAuto; -import frc.robot.Auto.ShootEightAuto; import frc.robot.Auto.CenterLemonAuto; import frc.robot.Auto.CenterToDepotAuto; import frc.robot.Auto.DepotShootingAuto; -import frc.robot.Auto.DriveTestAuto; import frc.robot.Constants.AutoConstants; import frc.robot.Constants.VisionConstants; import frc.robot.Constants.ShooterConstants; @@ -39,6 +37,8 @@ import frc.robot.Subsystems.IntakeSubsystem; import frc.robot.Subsystems.ShooterSubsystem; import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.UnusedAuto.DriveTestAuto; +import frc.robot.UnusedAuto.ShootEightAuto; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.DriverStation; diff --git a/src/main/java/frc/robot/Auto/DriveTestAuto.java b/src/main/java/frc/robot/UnusedAuto/DriveTestAuto.java similarity index 95% rename from src/main/java/frc/robot/Auto/DriveTestAuto.java rename to src/main/java/frc/robot/UnusedAuto/DriveTestAuto.java index f41b024..89a1e05 100644 --- a/src/main/java/frc/robot/Auto/DriveTestAuto.java +++ b/src/main/java/frc/robot/UnusedAuto/DriveTestAuto.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.Auto; +package frc.robot.UnusedAuto; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.Subsystems.IntakeSubsystem; diff --git a/src/main/java/frc/robot/Auto/ShootEightAuto.java b/src/main/java/frc/robot/UnusedAuto/ShootEightAuto.java similarity index 94% rename from src/main/java/frc/robot/Auto/ShootEightAuto.java rename to src/main/java/frc/robot/UnusedAuto/ShootEightAuto.java index d4dba86..f13b2ca 100644 --- a/src/main/java/frc/robot/Auto/ShootEightAuto.java +++ b/src/main/java/frc/robot/UnusedAuto/ShootEightAuto.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.Auto; +package frc.robot.UnusedAuto; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -36,6 +36,7 @@ public ShootEightAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubs shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ) ); From 7aa38bdb1ce41e57818b03cfcfa24ae70c4762d2 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Mon, 11 May 2026 16:57:07 -0500 Subject: [PATCH 75/96] deleted unused auto imports --- src/main/java/frc/robot/Constants.java | 6 ++---- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5719a69..60874f5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -155,12 +155,11 @@ public static final class AutoConstants { public enum AutoMode{ None, - DriveTestAuto, LeftLemonAuto, RightLemonAuto, LeftNeutralZoneAuto1, + LeftNeutralZoneAuto2, RightNeutralZoneAuto1, - ShootEightAuto, CenterLemonAuto, CenterToDepotAuto, DepotShootingAuto @@ -179,12 +178,11 @@ public static void initDashboard() { autoModeChooser.setDefaultOption("LeftLemonAuto", AutoMode.LeftLemonAuto); autoModeChooser.addOption("None", AutoMode.None); - autoModeChooser.addOption("DriveTestAuto", AutoMode.DriveTestAuto); - autoModeChooser.addOption("ShootEightAuto", AutoMode.ShootEightAuto); autoModeChooser.addOption("RightLemonAuto", AutoMode.RightLemonAuto); autoModeChooser.addOption("LeftLemonAuto", AutoMode.LeftLemonAuto); autoModeChooser.addOption("RightNeutralZoneAuto1", AutoMode.RightNeutralZoneAuto1); autoModeChooser.addOption("LeftNeutralZoneAuto1", AutoMode.LeftNeutralZoneAuto1); + autoModeChooser.addOption("LeftNeutralZoneAuto2", AutoMode.LeftNeutralZoneAuto2); autoModeChooser.addOption("CenterLemonAuto", AutoMode.CenterLemonAuto); autoModeChooser.addOption("CenterToDepotAuto", AutoMode.CenterToDepotAuto); autoModeChooser.addOption("DepotShootingAuto", AutoMode.DepotShootingAuto); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8f68a37..80ed626 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Auto.LeftLemonAuto; import frc.robot.Auto.LeftNeutralZoneAuto1; +import frc.robot.Auto.LeftNeutralZoneAuto2; import frc.robot.Auto.RightNeutralZoneAuto1; import frc.robot.Auto.RightLemonAuto; import frc.robot.Auto.CenterLemonAuto; @@ -240,12 +241,11 @@ public Command getAutonomousCommand() { return switch (selected) { case None -> Commands.none(); - case DriveTestAuto -> new DriveTestAuto(m_drive, m_intake, m_shooter); case LeftLemonAuto -> new LeftLemonAuto(m_drive, m_intake, m_shooter); case RightLemonAuto -> new RightLemonAuto(m_drive, m_intake, m_shooter); case RightNeutralZoneAuto1 -> new RightNeutralZoneAuto1 (m_drive, m_intake, m_shooter); + case LeftNeutralZoneAuto2 -> new LeftNeutralZoneAuto2 (m_drive, m_intake, m_shooter); case LeftNeutralZoneAuto1 -> new LeftNeutralZoneAuto1(m_drive, m_intake, m_shooter); - case ShootEightAuto -> new ShootEightAuto(m_drive, m_intake, m_shooter); case CenterLemonAuto -> new CenterLemonAuto(m_drive, m_intake, m_shooter); case CenterToDepotAuto -> new CenterToDepotAuto(m_drive, m_intake, m_shooter); case DepotShootingAuto -> new DepotShootingAuto(m_drive, m_intake, m_shooter); From d133bb4eb6a17343392d1610699e942e89f549dd Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Mon, 11 May 2026 17:33:38 -0500 Subject: [PATCH 76/96] edit leftneutralzone auto 2 --- .../java/frc/robot/Auto/LeftNeutralZoneAuto2.java | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java index 83f528b..af86132 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java @@ -19,7 +19,7 @@ public class LeftNeutralZoneAuto2 extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 2.0; + private static final double DRIVE_SPEED_MPS = 3.5; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; @@ -108,7 +108,15 @@ public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoot Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) ) - .repeatedly() + .repeatedly(), + + + //Go to the neutral zone a second time. + // Turn 20 degrees right + turnRelativeDegrees(drive, -20.0), + // Drive FORWARD (back to the trench) + driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS) + ) ); } From ca7b999312360b1c637510d4bbff4c35eabe3d58 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Mon, 11 May 2026 18:08:00 -0500 Subject: [PATCH 77/96] Second intake in the neutral zone --- .../frc/robot/Auto/LeftNeutralZoneAuto2.java | 80 +++++++++++++++++-- 1 file changed, 72 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java index af86132..c134832 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java @@ -33,6 +33,7 @@ public class LeftNeutralZoneAuto2 extends SequentialCommandGroup { //private static final double FORWARD_METERS_3 = 3.2; private static final double INTAKE_POWER = -0.75; + private static final double FEED_DURATION_SEC = 3.0; public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addRequirements(drive, intake, shooter); @@ -78,10 +79,80 @@ public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoot .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) .withTimeout(1.0), + + /*Run the intake, kicker, indexer, hopper, and intake arm together for a fixed time, + then stop the feeding mechanisms so another driving path can be added.*/ // Keep intake running while the intake arm cycles up/down during shooting. Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), Commands.parallel( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + }, shooter) + ) + ), + + Commands.waitSeconds(1), + + // While feeding, continuously move the intake arm up/down for the fixed feed time. + Commands.sequence( + Commands.runOnce(intake::lowerIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), + Commands.runOnce(intake::raiseIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) + ) + .repeatedly() + .withTimeout(FEED_DURATION_SEC), + + + + + //Go to the neutral zone a second time. + // Turn 20 degrees right + turnRelativeDegrees(drive, -20.0), + + // Drive FORWARD (back to the trench) + driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), + + // Turn 90 degrees right + turnRelativeDegrees(drive, -90.0), + + // Drive forwards 3m (intake still on). + driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + + // Drive backwards 3m (intake still on). + driveDistanceMeters(drive, -FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Stop intake at the end. + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + // Turn 90 degrees left + turnRelativeDegrees(drive, 90.0), + + // Drive backward (back to the trench) + driveDistanceMeters(drive, -BACKWARD_METERS_2, DRIVE_SPEED_MPS), + + // Turn 20 degrees left + turnRelativeDegrees(drive, 20.0), + + // Bring hood up to HIGH angle. + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + + Commands.parallel( // Start kicker first, then start indexer 1 second later (kicker keeps running). Commands.sequence( Commands.run(() -> { @@ -108,14 +179,7 @@ public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoot Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) ) - .repeatedly(), - - - //Go to the neutral zone a second time. - // Turn 20 degrees right - turnRelativeDegrees(drive, -20.0), - // Drive FORWARD (back to the trench) - driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS) + .repeatedly() ) ); From f0f23bce3ca2221ab39954d9040431e6cf3d2474 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Mon, 11 May 2026 19:28:48 -0500 Subject: [PATCH 78/96] OMG --- src/main/java/frc/robot/Auto/CenterLemonAuto.java | 4 ++-- src/main/java/frc/robot/Auto/LeftLemonAuto.java | 2 +- src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java | 2 +- src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java | 9 ++++----- src/main/java/frc/robot/Auto/RightLemonAuto.java | 2 +- src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java | 2 +- 6 files changed, 10 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Auto/CenterLemonAuto.java b/src/main/java/frc/robot/Auto/CenterLemonAuto.java index 028cc56..6fc41fd 100644 --- a/src/main/java/frc/robot/Auto/CenterLemonAuto.java +++ b/src/main/java/frc/robot/Auto/CenterLemonAuto.java @@ -20,7 +20,7 @@ public CenterLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSub Commands.waitSeconds(2), new InstantCommand(()->drive.drive(0,0,0, false),drive), - Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.MED), shooter), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), shooter), Commands.runOnce(() -> { shooter.runIndexer(false); shooter.runKicker(false); @@ -40,7 +40,7 @@ public CenterLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSub shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); - shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ) ); diff --git a/src/main/java/frc/robot/Auto/LeftLemonAuto.java b/src/main/java/frc/robot/Auto/LeftLemonAuto.java index 6c2f328..e7580a1 100644 --- a/src/main/java/frc/robot/Auto/LeftLemonAuto.java +++ b/src/main/java/frc/robot/Auto/LeftLemonAuto.java @@ -54,7 +54,7 @@ public LeftLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsy shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); - shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ) ); diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index 8d99d78..4c30f5f 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -91,7 +91,7 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot Commands.run(() -> { shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); - shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ), diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java index c134832..90f35a2 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java @@ -19,7 +19,7 @@ public class LeftNeutralZoneAuto2 extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 3.5; + private static final double DRIVE_SPEED_MPS = 2.0; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; @@ -33,7 +33,7 @@ public class LeftNeutralZoneAuto2 extends SequentialCommandGroup { //private static final double FORWARD_METERS_3 = 3.2; private static final double INTAKE_POWER = -0.75; - private static final double FEED_DURATION_SEC = 3.0; + private static final double FEED_DURATION_SEC = 1.5; public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addRequirements(drive, intake, shooter); @@ -95,7 +95,7 @@ public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoot Commands.run(() -> { shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); - shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ) ), @@ -113,7 +113,6 @@ public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoot Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) ) - .repeatedly() .withTimeout(FEED_DURATION_SEC), @@ -162,7 +161,7 @@ public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoot Commands.run(() -> { shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); - shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ), diff --git a/src/main/java/frc/robot/Auto/RightLemonAuto.java b/src/main/java/frc/robot/Auto/RightLemonAuto.java index 800cd31..ff8aa29 100644 --- a/src/main/java/frc/robot/Auto/RightLemonAuto.java +++ b/src/main/java/frc/robot/Auto/RightLemonAuto.java @@ -54,7 +54,7 @@ public RightLemonAuto(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubs shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED); shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); - shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ) ); diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java index 7583483..1c26b99 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java @@ -89,7 +89,7 @@ public RightNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoo Commands.run(() -> { shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); - shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) ), From ef80c69d86116149be83d4420fd2043fdfc90a0a Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Tue, 12 May 2026 16:43:49 -0500 Subject: [PATCH 79/96] new offsets and sparkmax id --- src/main/java/frc/robot/Constants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 60874f5..2f99ad0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -128,10 +128,10 @@ public record ModuleData( ){} public static ModuleData[] moduleData = { - new ModuleData(6, 5, 7, 39.90, FRONT_LEFT, driveInvert, angleInvert), //Mod 0 Front left - new ModuleData(9, 8, 10, 46.93, FRONT_RIGHT, driveInvert, angleInvert), //Mod 1 Front right - new ModuleData(12, 11, 13, 31.72, BACK_RIGHT, driveInvert, angleInvert), //Mod 2 Back right - new ModuleData(15, 14, 16, 7.11, BACK_LEFT, driveInvert, angleInvert) //Mod 3 Back left + new ModuleData(6, 5, 7, 29.88, FRONT_LEFT, driveInvert, angleInvert), //Mod 0 Front left + new ModuleData(9, 31, 10, 42.09, FRONT_RIGHT, driveInvert, angleInvert), //Mod 1 Front right + new ModuleData(12, 11, 13, 41.39, BACK_RIGHT, driveInvert, angleInvert), //Mod 2 Back right + new ModuleData(15, 14, 16, 7.82, BACK_LEFT, driveInvert, angleInvert) //Mod 3 Back left }; } From 19cf279efcc0d8b4507cc8ed330023738c541572 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Tue, 12 May 2026 17:16:59 -0500 Subject: [PATCH 80/96] e --- .../java/frc/robot/Auto/LeftNeutralZoneAuto1.java | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java index 4c30f5f..2248606 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto1.java @@ -27,8 +27,10 @@ public class LeftNeutralZoneAuto1 extends SequentialCommandGroup { private static final double TURN_TIMEOUT_SEC = 2.5; private static final double BACKWARD_METERS_1 = 3.6; - private static final double BACKWARD_METERS_2 = 4.3; - private static final double FORWARD_METERS_1 = 2.0; + private static final double BACKWARD_METERS_2 = 4.5; + private static final double BACKWARD_METERS_3 = 2.6; + + private static final double FORWARD_METERS_1 = 2.5; //private static final double FORWARD_METERS_2 = 1.0; //private static final double FORWARD_METERS_3 = 3.2; @@ -46,12 +48,12 @@ public LeftNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoot // Turn 90 degrees left. turnRelativeDegrees(drive, 90.0), - // Drive forward 2m while starting intake (intake stays on for the rest of auto). + // Drive forward 3m while starting intake (intake stays on for the rest of auto). Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), - // Drive forward 2m (intake still on). - driveDistanceMeters(drive, -FORWARD_METERS_1, DRIVE_SPEED_MPS), + // Drive forward 3m (intake still on). + driveDistanceMeters(drive, -BACKWARD_METERS_3, DRIVE_SPEED_MPS), // Stop intake at the end. Commands.runOnce(() -> intake.setIntakePower(0.0), intake), From 033bc72ddca59492e58df07453ef148d4c96355c Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Tue, 12 May 2026 17:23:34 -0500 Subject: [PATCH 81/96] auto align distance speed to 0 --- src/main/java/frc/robot/Command/SimpleAutoAlign.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Command/SimpleAutoAlign.java b/src/main/java/frc/robot/Command/SimpleAutoAlign.java index 94eb07a..711c256 100644 --- a/src/main/java/frc/robot/Command/SimpleAutoAlign.java +++ b/src/main/java/frc/robot/Command/SimpleAutoAlign.java @@ -34,7 +34,7 @@ public class SimpleAutoAlign extends Command { //Tolerance and public static final double DISTANCE_TOLERANCE_METERS = 0.08; public static final double ROTATION_TOLERANCE_DEG = 1.5; - public static final double MAX_FORWARD_SPEED_MPS = SwerveConstants.maxSpeed; + public static final double MAX_FORWARD_SPEED_MPS = 0; public static final double MAX_ROTATION_SPEED_RAD_PER_SEC = 2.5; public static final double MIN_DISTANCE_CALC_ANGLE_DEG = 1.0; public static final int SETTLE_CYCLES_REQUIRED = 10; From bdb0512919225fa254e51dc89e8aa50600e4e719 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Tue, 12 May 2026 17:29:29 -0500 Subject: [PATCH 82/96] update neutral zone auto 2 --- src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java index 90f35a2..eed4d48 100644 --- a/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java +++ b/src/main/java/frc/robot/Auto/LeftNeutralZoneAuto2.java @@ -97,7 +97,6 @@ public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoot shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); }, shooter) - ) ), Commands.waitSeconds(1), @@ -113,7 +112,8 @@ public LeftNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoot Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) ) - .withTimeout(FEED_DURATION_SEC), + .repeatedly() + ).withTimeout(FEED_DURATION_SEC), From 0e72b84256efdfe84c94557785cc68803e6519c5 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Tue, 12 May 2026 17:38:45 -0500 Subject: [PATCH 83/96] commented distance alignment out --- .../frc/robot/Command/SimpleAutoAlign.java | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/Command/SimpleAutoAlign.java b/src/main/java/frc/robot/Command/SimpleAutoAlign.java index 711c256..fe139ad 100644 --- a/src/main/java/frc/robot/Command/SimpleAutoAlign.java +++ b/src/main/java/frc/robot/Command/SimpleAutoAlign.java @@ -139,21 +139,22 @@ public void execute() { return; } - double ty = getVerticalOffsetToTarget(); - double distance = getDistanceToTargetMeters(ty); - boolean distanceIsValid = Double.isFinite(distance); - double filteredDistance = distanceIsValid ? filterDistanceMeters(distance) : Double.NaN; + //double ty = getVerticalOffsetToTarget(); + //double distance = getDistanceToTargetMeters(ty); + //boolean distanceIsValid = Double.isFinite(distance); + //double filteredDistance = distanceIsValid ? filterDistanceMeters(distance) : Double.NaN; double desiredAlignmentAngle = getDesiredAlignmentAngle(tagId); double rotationSpeed = rotationController.calculate(offset, desiredAlignmentAngle); double rotationError = offset - desiredAlignmentAngle; - double distanceError = distanceIsValid ? filteredDistance - TARGET_DISTANCE_METERS : Double.NaN; + //double distanceError = distanceIsValid ? filteredDistance - TARGET_DISTANCE_METERS : Double.NaN; boolean withinRotationTolerance = Math.abs(rotationError) <= ROTATION_TOLERANCE_DEG; - boolean withinDistanceTolerance = distanceIsValid && Math.abs(distanceError) <= DISTANCE_TOLERANCE_METERS; + //boolean withinDistanceTolerance = distanceIsValid && Math.abs(distanceError) <= DISTANCE_TOLERANCE_METERS; - if (withinRotationTolerance && withinDistanceTolerance){ + //if (withinRotationTolerance && withinDistanceTolerance){ + if (withinRotationTolerance){ settledCycles++; } else { settledCycles = 0; @@ -167,13 +168,13 @@ public void execute() { } double driveSpeed = 0.0; - if (distanceIsValid && Math.abs (rotationError) <= UNLOCK_ROTATION_ERROR_DEG &&! withinDistanceTolerance){ + /*if (distanceIsValid && Math.abs (rotationError) <= UNLOCK_ROTATION_ERROR_DEG &&! withinDistanceTolerance){ driveSpeed = distanceController.calculate(0.0, distanceError); } - + */ - driveSpeed = MathUtil.clamp(driveSpeed, -MAX_FORWARD_SPEED_MPS, MAX_FORWARD_SPEED_MPS); - driveSpeed = distanceSpeedLimiter.calculate(driveSpeed); + //driveSpeed = MathUtil.clamp(driveSpeed, -MAX_FORWARD_SPEED_MPS, MAX_FORWARD_SPEED_MPS); + //driveSpeed = distanceSpeedLimiter.calculate(driveSpeed); rotationSpeed = MathUtil.clamp(rotationSpeed, -MAX_ROTATION_SPEED_RAD_PER_SEC, MAX_ROTATION_SPEED_RAD_PER_SEC); swerveSubsystem.driveFromChassisSpeeds(new ChassisSpeeds(0, driveSpeed, rotationSpeed), false); From d2a97d961f923bca04383f0f23c4e383535c964b Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Tue, 12 May 2026 17:52:39 -0500 Subject: [PATCH 84/96] rezero --- src/main/java/frc/robot/Constants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2f99ad0..ff7d071 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -128,10 +128,10 @@ public record ModuleData( ){} public static ModuleData[] moduleData = { - new ModuleData(6, 5, 7, 29.88, FRONT_LEFT, driveInvert, angleInvert), //Mod 0 Front left - new ModuleData(9, 31, 10, 42.09, FRONT_RIGHT, driveInvert, angleInvert), //Mod 1 Front right - new ModuleData(12, 11, 13, 41.39, BACK_RIGHT, driveInvert, angleInvert), //Mod 2 Back right - new ModuleData(15, 14, 16, 7.82, BACK_LEFT, driveInvert, angleInvert) //Mod 3 Back left + new ModuleData(6, 5, 7, 34.18, FRONT_LEFT, driveInvert, angleInvert), //Mod 0 Front left + new ModuleData(9, 31, 10, 44.03, FRONT_RIGHT, driveInvert, angleInvert), //Mod 1 Front right + new ModuleData(12, 11, 13, 28.21, BACK_RIGHT, driveInvert, angleInvert), //Mod 2 Back right + new ModuleData(15, 14, 16, 8.87, BACK_LEFT, driveInvert, angleInvert) //Mod 3 Back left }; } From 1ed2af7cfb8a92c9d4d5307fc373c94d5592863b Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Tue, 12 May 2026 18:19:13 -0500 Subject: [PATCH 85/96] right neutral zone auto 2 with a 2nd neutral zone path, auto align april tags --- .../frc/robot/Auto/RightNeutralZoneAuto2.java | 208 ++++++++++++++++++ .../frc/robot/Command/SimpleAutoAlign.java | 2 +- src/main/java/frc/robot/Constants.java | 2 + src/main/java/frc/robot/RobotContainer.java | 2 + 4 files changed, 213 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java new file mode 100644 index 0000000..935a9ac --- /dev/null +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java @@ -0,0 +1,208 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Auto; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.ShooterConstants; +import frc.robot.Constants.SwerveConstants; +import frc.robot.Subsystems.SwerveSubsystem; +import frc.robot.Subsystems.IntakeSubsystem; +import frc.robot.Subsystems.ShooterSubsystem; + +import java.util.concurrent.atomic.AtomicReference; + + +public class RightNeutralZoneAuto2 extends SequentialCommandGroup { + private static final double DRIVE_SPEED_MPS = 2.0; + private static final double DRIVE_HEADING_P = 3.0; + private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; + private static final double TURN_P = 4.0; + private static final double TURN_TOLERANCE_DEG = 3.0; + private static final double TURN_TIMEOUT_SEC = 2.5; + + private static final double BACKWARD_METERS_1 = 3.6; + private static final double BACKWARD_METERS_2 = 4.3; + private static final double FORWARD_METERS_1 = 3.0; + + private static final double INTAKE_POWER = -0.75; + private static final double FEED_DURATION_SEC = 3.0; + + public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { + addRequirements(drive, intake, shooter); + + addCommands( + Commands.runOnce(intake::lowerIntake, intake), + + // Drive backwards 3.6m. + driveDistanceMeters(drive, -BACKWARD_METERS_1, DRIVE_SPEED_MPS), + + // Turn 90 degrees right. + turnRelativeDegrees(drive, -90.0), + + // Drive forward 2m while starting intake (intake stays on for the rest of auto). + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Drive back 2m (intake still on). + driveDistanceMeters(drive, -FORWARD_METERS_1, DRIVE_SPEED_MPS), + + // Turn 90 degrees left (intake still on). + turnRelativeDegrees(drive, 90.0), + + // Stop intake at the end. + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + // Drive forward (back to the trench) + driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), + + // Turn 13 degrees right + turnRelativeDegrees(drive,-13.0), + + // Bring hood up to HIGH angle. + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + + // Shooter + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(1.0), + + // Keep intake running while the intake arm cycles up/down during shooting. + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + + Commands.parallel( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + }, shooter) + ), + + Commands.waitSeconds(2), + + // While shooting/indexing, continuously move the intake arm up/down. + Commands.sequence( + Commands.runOnce(intake::lowerIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), + Commands.runOnce(intake::raiseIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) + ) + .repeatedly() + ).withTimeout(FEED_DURATION_SEC), + + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), shooter), + + //Turn 13 degrees left + turnRelativeDegrees(drive, 13), + + driveDistanceMeters(drive, -BACKWARD_METERS_2, DRIVE_SPEED_MPS), + + turnRelativeDegrees(drive,-90), + + Commands.runOnce(()-> intake.setIntakePower(INTAKE_POWER), intake), + driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS) + ); + } + + private static edu.wpi.first.wpilibj2.command.Command driveDistanceMeters( + SwerveSubsystem drive, + double distanceMeters, + double speedMps) { + double clampedSpeedMps = MathUtil.clamp(Math.abs(speedMps), 0.0, SwerveConstants.maxSpeed); + double commandedSpeedMps = Math.copySign(clampedSpeedMps, distanceMeters); + double distanceAbsMeters = Math.abs(distanceMeters); + + AtomicReference startPositions = new AtomicReference<>(); + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> { + startPositions.set(drive.getPositions()); + startYawRad[0] = drive.getYaw().getRadians(); + }, drive), + Commands.runEnd( + () -> { + double errorRad = MathUtil.angleModulus(startYawRad[0] - drive.getYaw().getRadians()); + double maxOmegaRadPerSec = + Math.min(DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC, SwerveConstants.maxAngularVelocity); + double omegaRadPerSec = + MathUtil.clamp(errorRad * DRIVE_HEADING_P, -maxOmegaRadPerSec, maxOmegaRadPerSec); + drive.drive(commandedSpeedMps, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until( + () -> getAverageWheelDeltaMeters(startPositions.get(), drive.getPositions()) >= distanceAbsMeters) + .withTimeout(distanceAbsMeters / Math.max(0.1, Math.abs(commandedSpeedMps)) + 1.0) + ); + } + + private static double getAverageWheelDeltaMeters( + SwerveModulePosition[] startPositions, + SwerveModulePosition[] currentPositions) { + if (startPositions == null || currentPositions == null) { + return 0.0; + } + + int count = Math.min(startPositions.length, currentPositions.length); + if (count <= 0) { + return 0.0; + } + + double sum = 0.0; + for (int i = 0; i < count; i++) { + sum += Math.abs(currentPositions[i].distanceMeters - startPositions[i].distanceMeters); + } + return sum / count; + } + + private static edu.wpi.first.wpilibj2.command.Command turnRelativeDegrees( + SwerveSubsystem drive, + double deltaDegrees) { + final double[] startYawRad = new double[1]; + + return Commands.sequence( + Commands.runOnce(() -> startYawRad[0] = drive.getYaw().getRadians(), drive), + Commands.runEnd( + () -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + double omegaRadPerSec = + MathUtil.clamp( + errorRad * TURN_P, + -SwerveConstants.maxAngularVelocity, + SwerveConstants.maxAngularVelocity); + drive.drive(0, 0, omegaRadPerSec, false); + }, + () -> drive.drive(0, 0, 0, false), + drive) + .until(() -> { + double targetYawRad = startYawRad[0] + Math.toRadians(deltaDegrees); + double errorRad = MathUtil.angleModulus(targetYawRad - drive.getYaw().getRadians()); + return Math.abs(errorRad) < Math.toRadians(TURN_TOLERANCE_DEG); + }) + .withTimeout(TURN_TIMEOUT_SEC) + + ); + } +} diff --git a/src/main/java/frc/robot/Command/SimpleAutoAlign.java b/src/main/java/frc/robot/Command/SimpleAutoAlign.java index fe139ad..5007bc0 100644 --- a/src/main/java/frc/robot/Command/SimpleAutoAlign.java +++ b/src/main/java/frc/robot/Command/SimpleAutoAlign.java @@ -67,7 +67,7 @@ private int getTagId() { } private boolean isSupportedTag(int tagId) { - return tagId == 10 || tagId == 9 || tagId == 25 || tagId == 11 || tagId == 27 || tagId == 8 || tagId == 24; + return tagId == 10 || tagId == 11 ||tagId == 26 || tagId == 27 || tagId == 8 || tagId == 24; } private double getDesiredAlignmentAngle(int tagId) { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ff7d071..e0f8e18 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -160,6 +160,7 @@ public enum AutoMode{ LeftNeutralZoneAuto1, LeftNeutralZoneAuto2, RightNeutralZoneAuto1, + RightNeutralZoneAuto2, CenterLemonAuto, CenterToDepotAuto, DepotShootingAuto @@ -181,6 +182,7 @@ public static void initDashboard() { autoModeChooser.addOption("RightLemonAuto", AutoMode.RightLemonAuto); autoModeChooser.addOption("LeftLemonAuto", AutoMode.LeftLemonAuto); autoModeChooser.addOption("RightNeutralZoneAuto1", AutoMode.RightNeutralZoneAuto1); + autoModeChooser.addOption("RightNeutralZoneAuto2", AutoMode.RightNeutralZoneAuto2); autoModeChooser.addOption("LeftNeutralZoneAuto1", AutoMode.LeftNeutralZoneAuto1); autoModeChooser.addOption("LeftNeutralZoneAuto2", AutoMode.LeftNeutralZoneAuto2); autoModeChooser.addOption("CenterLemonAuto", AutoMode.CenterLemonAuto); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 80ed626..0f0a47b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,6 +24,7 @@ import frc.robot.Auto.LeftNeutralZoneAuto1; import frc.robot.Auto.LeftNeutralZoneAuto2; import frc.robot.Auto.RightNeutralZoneAuto1; +import frc.robot.Auto.RightNeutralZoneAuto2; import frc.robot.Auto.RightLemonAuto; import frc.robot.Auto.CenterLemonAuto; import frc.robot.Auto.CenterToDepotAuto; @@ -244,6 +245,7 @@ public Command getAutonomousCommand() { case LeftLemonAuto -> new LeftLemonAuto(m_drive, m_intake, m_shooter); case RightLemonAuto -> new RightLemonAuto(m_drive, m_intake, m_shooter); case RightNeutralZoneAuto1 -> new RightNeutralZoneAuto1 (m_drive, m_intake, m_shooter); + case RightNeutralZoneAuto2 -> new RightNeutralZoneAuto2 (m_drive, m_intake, m_shooter); case LeftNeutralZoneAuto2 -> new LeftNeutralZoneAuto2 (m_drive, m_intake, m_shooter); case LeftNeutralZoneAuto1 -> new LeftNeutralZoneAuto1(m_drive, m_intake, m_shooter); case CenterLemonAuto -> new CenterLemonAuto(m_drive, m_intake, m_shooter); From 7d28c1856875817b2fcfc40d0329fa703c24bdc7 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Tue, 12 May 2026 18:25:15 -0500 Subject: [PATCH 86/96] ok --- .../java/frc/robot/Auto/RightNeutralZoneAuto2.java | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java index 935a9ac..6f495bf 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java @@ -110,6 +110,10 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo .repeatedly() ).withTimeout(FEED_DURATION_SEC), + Commands.runOnce(()->shooter.setShooterSpeed(0.0), shooter), + + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), shooter), //Turn 13 degrees left @@ -120,7 +124,14 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo turnRelativeDegrees(drive,-90), Commands.runOnce(()-> intake.setIntakePower(INTAKE_POWER), intake), - driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS) + driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + + driveDistanceMeters(drive, -FORWARD_METERS_1, DRIVE_SPEED_MPS), + + turnRelativeDegrees(drive, 90.0), + + Commands.runOnce(() -> intake.setIntakePower(0.0), intake), + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive) ); } From 60a966250d3e56b8b97b7ffabe5b52962a870c8f Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Tue, 12 May 2026 18:30:04 -0500 Subject: [PATCH 87/96] go with it --- .../java/frc/robot/Auto/RightNeutralZoneAuto2.java | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java index 6f495bf..e5d76e2 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java @@ -31,7 +31,7 @@ public class RightNeutralZoneAuto2 extends SequentialCommandGroup { private static final double FORWARD_METERS_1 = 3.0; private static final double INTAKE_POWER = -0.75; - private static final double FEED_DURATION_SEC = 3.0; + private static final double FEED_DURATION_SEC = 5.0; public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addRequirements(drive, intake, shooter); @@ -110,6 +110,10 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo .repeatedly() ).withTimeout(FEED_DURATION_SEC), + Commands.runOnce(()->shooter.setKickerSpeed(0.0), shooter), + + Commands.runOnce(()->shooter.setIndexerSpeed(0.0), shooter), + Commands.runOnce(()->shooter.setShooterSpeed(0.0), shooter), Commands.runOnce(() -> intake.setIntakePower(0.0), intake), @@ -131,7 +135,11 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo turnRelativeDegrees(drive, 90.0), Commands.runOnce(() -> intake.setIntakePower(0.0), intake), - Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive) + Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), + + driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), + turnRelativeDegrees(drive,-13.0) + ); } From bdc5ad69a5adacf97e107cbc842324dd7a732be7 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Tue, 12 May 2026 18:36:43 -0500 Subject: [PATCH 88/96] !@#$%^&*()_ --- .../frc/robot/Auto/RightNeutralZoneAuto2.java | 54 ++++++++++++++++++- 1 file changed, 52 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java index e5d76e2..c0dbe04 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java @@ -19,7 +19,7 @@ public class RightNeutralZoneAuto2 extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 2.0; + private static final double DRIVE_SPEED_MPS = 2.5; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; @@ -110,6 +110,8 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo .repeatedly() ).withTimeout(FEED_DURATION_SEC), + Commands.runOnce(intake::lowerIntake, intake), + Commands.runOnce(()->shooter.setKickerSpeed(0.0), shooter), Commands.runOnce(()->shooter.setIndexerSpeed(0.0), shooter), @@ -138,7 +140,55 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), - turnRelativeDegrees(drive,-13.0) + turnRelativeDegrees(drive,-13.0), + + + + + + // Bring hood up to HIGH angle. + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + + // Shooter + Commands.runOnce(() -> { + shooter.runIndexer(false); + shooter.runKicker(false); + }, shooter), + Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) + .withTimeout(1.0), + + // Keep intake running while the intake arm cycles up/down during shooting. + Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), + + Commands.parallel( + // Start kicker first, then start indexer 1 second later (kicker keeps running). + Commands.sequence( + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(0.0); + }, shooter).withTimeout(1.0), + Commands.run(() -> { + shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); + shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); + //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); + }, shooter) + ), + + Commands.waitSeconds(2), + + // While shooting/indexing, continuously move the intake arm up/down. + Commands.sequence( + Commands.runOnce(intake::lowerIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), + Commands.runOnce(intake::raiseIntake, intake), + Commands.waitUntil(() -> + Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) + <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) + ) + .repeatedly()) ); } From 4c2436309e06304ca8717833ec21cfe9ded80d92 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Tue, 12 May 2026 19:16:11 -0500 Subject: [PATCH 89/96] asdghjkl;' --- .../frc/robot/Auto/RightNeutralZoneAuto1.java | 7 +++---- .../frc/robot/Auto/RightNeutralZoneAuto2.java | 15 +++++++-------- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java index 1c26b99..9b57383 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java @@ -19,7 +19,7 @@ public class RightNeutralZoneAuto1 extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 2.0; + private static final double DRIVE_SPEED_MPS = 3.0; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; @@ -28,7 +28,7 @@ public class RightNeutralZoneAuto1 extends SequentialCommandGroup { private static final double BACKWARD_METERS_1 = 3.6; private static final double BACKWARD_METERS_2 = 4.3; - private static final double FORWARD_METERS_1 = 2.0; + private static final double FORWARD_METERS_1 = 3.0; private static final double INTAKE_POWER = -0.75; @@ -73,8 +73,7 @@ public RightNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoo shooter.runKicker(false); }, shooter), Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) - .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) - .withTimeout(1.0), + .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM), // Keep intake running while the intake arm cycles up/down during shooting. Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java index c0dbe04..3364fb0 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java @@ -19,7 +19,7 @@ public class RightNeutralZoneAuto2 extends SequentialCommandGroup { - private static final double DRIVE_SPEED_MPS = 2.5; + private static final double DRIVE_SPEED_MPS = 3.0; private static final double DRIVE_HEADING_P = 3.0; private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; @@ -29,6 +29,7 @@ public class RightNeutralZoneAuto2 extends SequentialCommandGroup { private static final double BACKWARD_METERS_1 = 3.6; private static final double BACKWARD_METERS_2 = 4.3; private static final double FORWARD_METERS_1 = 3.0; + private static final double FORWARD_METERS_2 = 4.0; private static final double INTAKE_POWER = -0.75; private static final double FEED_DURATION_SEC = 5.0; @@ -110,8 +111,6 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo .repeatedly() ).withTimeout(FEED_DURATION_SEC), - Commands.runOnce(intake::lowerIntake, intake), - Commands.runOnce(()->shooter.setKickerSpeed(0.0), shooter), Commands.runOnce(()->shooter.setIndexerSpeed(0.0), shooter), @@ -130,9 +129,9 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo turnRelativeDegrees(drive,-90), Commands.runOnce(()-> intake.setIntakePower(INTAKE_POWER), intake), - driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + driveDistanceMeters(drive, FORWARD_METERS_2, DRIVE_SPEED_MPS), - driveDistanceMeters(drive, -FORWARD_METERS_1, DRIVE_SPEED_MPS), + driveDistanceMeters(drive, -FORWARD_METERS_2, DRIVE_SPEED_MPS), turnRelativeDegrees(drive, 90.0), @@ -148,7 +147,7 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo // Bring hood up to HIGH angle. Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), - + // Shooter Commands.runOnce(() -> { shooter.runIndexer(false); @@ -188,8 +187,8 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) ) - .repeatedly()) - + .repeatedly() + ) ); } From 9787836ca2f289b4b8689097121696b35094586e Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Tue, 12 May 2026 19:25:10 -0500 Subject: [PATCH 90/96] notime --- .../frc/robot/Auto/RightNeutralZoneAuto2.java | 31 ++++++++++--------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java index c0dbe04..acc7596 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java @@ -62,11 +62,11 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo // Drive forward (back to the trench) driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), - // Turn 13 degrees right - turnRelativeDegrees(drive,-13.0), - - // Bring hood up to HIGH angle. - Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + // Turn 13 degrees right while bringing the hood up to avoid an extra pause. + Commands.parallel( + turnRelativeDegrees(drive,-13.0), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter) + ), // Shooter Commands.runOnce(() -> { @@ -120,10 +120,11 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo Commands.runOnce(() -> intake.setIntakePower(0.0), intake), - Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), shooter), - - //Turn 13 degrees left - turnRelativeDegrees(drive, 13), + // Bring the hood down while turning 13 degrees left to avoid an extra pause. + Commands.parallel( + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), shooter), + turnRelativeDegrees(drive, 13) + ), driveDistanceMeters(drive, -BACKWARD_METERS_2, DRIVE_SPEED_MPS), @@ -140,14 +141,14 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), - turnRelativeDegrees(drive,-13.0), - - - - // Bring hood up to HIGH angle. - Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + + // Turn 13 degrees right while bringing the hood up to avoid an extra pause. + Commands.parallel( + turnRelativeDegrees(drive,-13.0), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter) + ), // Shooter Commands.runOnce(() -> { From 13b4834703a728aefaa49574796ced3dc0cedcfa Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Wed, 13 May 2026 09:04:58 -0500 Subject: [PATCH 91/96] Revert "notime" This reverts commit 9787836ca2f289b4b8689097121696b35094586e. --- .../frc/robot/Auto/RightNeutralZoneAuto2.java | 31 +++++++++---------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java index acc7596..c0dbe04 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java @@ -62,11 +62,11 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo // Drive forward (back to the trench) driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), - // Turn 13 degrees right while bringing the hood up to avoid an extra pause. - Commands.parallel( - turnRelativeDegrees(drive,-13.0), - Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter) - ), + // Turn 13 degrees right + turnRelativeDegrees(drive,-13.0), + + // Bring hood up to HIGH angle. + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), // Shooter Commands.runOnce(() -> { @@ -120,11 +120,10 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo Commands.runOnce(() -> intake.setIntakePower(0.0), intake), - // Bring the hood down while turning 13 degrees left to avoid an extra pause. - Commands.parallel( - Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), shooter), - turnRelativeDegrees(drive, 13) - ), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), shooter), + + //Turn 13 degrees left + turnRelativeDegrees(drive, 13), driveDistanceMeters(drive, -BACKWARD_METERS_2, DRIVE_SPEED_MPS), @@ -141,14 +140,14 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), + turnRelativeDegrees(drive,-13.0), - - // Turn 13 degrees right while bringing the hood up to avoid an extra pause. - Commands.parallel( - turnRelativeDegrees(drive,-13.0), - Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter) - ), + + + + // Bring hood up to HIGH angle. + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), // Shooter Commands.runOnce(() -> { From 59da3ea0941c247398e4c9c15cc1a58b19371935 Mon Sep 17 00:00:00 2001 From: Nonochen0104 Date: Thu, 14 May 2026 22:15:56 -0500 Subject: [PATCH 92/96] Auto update --- .../frc/robot/Auto/RightNeutralZoneAuto1.java | 12 +-- .../frc/robot/Auto/RightNeutralZoneAuto2.java | 91 +++++-------------- 2 files changed, 31 insertions(+), 72 deletions(-) diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java index 1c26b99..fd9e4bd 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto1.java @@ -24,7 +24,7 @@ public class RightNeutralZoneAuto1 extends SequentialCommandGroup { private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; private static final double TURN_TOLERANCE_DEG = 3.0; - private static final double TURN_TIMEOUT_SEC = 2.5; + private static final double TURN_TIMEOUT_SEC = 1.0; private static final double BACKWARD_METERS_1 = 3.6; private static final double BACKWARD_METERS_2 = 4.3; @@ -61,11 +61,11 @@ public RightNeutralZoneAuto1(SwerveSubsystem drive, IntakeSubsystem intake, Shoo // Drive forward (back to the trench) driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), - // Turn 13 degrees right - turnRelativeDegrees(drive,-13.0), - - // Bring hood up to HIGH angle. - Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + // Turn 13 degrees right & bring shooter hood to HIGH + Commands.parallel( + turnRelativeDegrees(drive,-13.0), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter) + ), // Shooter Commands.runOnce(() -> { diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java index c0dbe04..891e129 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java @@ -29,9 +29,10 @@ public class RightNeutralZoneAuto2 extends SequentialCommandGroup { private static final double BACKWARD_METERS_1 = 3.6; private static final double BACKWARD_METERS_2 = 4.3; private static final double FORWARD_METERS_1 = 3.0; + private static final double FORWARD_METERS_2 = 4.0; private static final double INTAKE_POWER = -0.75; - private static final double FEED_DURATION_SEC = 5.0; + private static final double FEED_DURATION_SEC = 13.0; public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addRequirements(drive, intake, shooter); @@ -62,11 +63,11 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo // Drive forward (back to the trench) driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), - // Turn 13 degrees right - turnRelativeDegrees(drive,-13.0), - - // Bring hood up to HIGH angle. - Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), + // Turn 13 degrees right & bring shooter hood to HIGH + Commands.parallel( + turnRelativeDegrees(drive,-13.0), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter) + ), // Shooter Commands.runOnce(() -> { @@ -110,17 +111,18 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo .repeatedly() ).withTimeout(FEED_DURATION_SEC), - Commands.runOnce(intake::lowerIntake, intake), - - Commands.runOnce(()->shooter.setKickerSpeed(0.0), shooter), - - Commands.runOnce(()->shooter.setIndexerSpeed(0.0), shooter), - - Commands.runOnce(()->shooter.setShooterSpeed(0.0), shooter), - - Commands.runOnce(() -> intake.setIntakePower(0.0), intake), - - Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW), shooter), + Commands.parallel( + Commands.runOnce(() -> { + intake.lowerIntake(); + intake.setIntakePower(0.0); + }, intake), + Commands.runOnce(() -> { + shooter.setKickerSpeed(0.0); + shooter.setIndexerSpeed(0.0); + shooter.setShooterSpeed(0.0); + shooter.setHoodAngle(ShooterSubsystem.HoodAngle.LOW); + }, shooter) + ), //Turn 13 degrees left turnRelativeDegrees(drive, 13), @@ -130,9 +132,9 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo turnRelativeDegrees(drive,-90), Commands.runOnce(()-> intake.setIntakePower(INTAKE_POWER), intake), - driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), + driveDistanceMeters(drive, FORWARD_METERS_2, DRIVE_SPEED_MPS), - driveDistanceMeters(drive, -FORWARD_METERS_1, DRIVE_SPEED_MPS), + driveDistanceMeters(drive, -FORWARD_METERS_2, DRIVE_SPEED_MPS), turnRelativeDegrees(drive, 90.0), @@ -140,56 +142,13 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), - turnRelativeDegrees(drive,-13.0), - - - - // Bring hood up to HIGH angle. - Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter), - - // Shooter - Commands.runOnce(() -> { - shooter.runIndexer(false); - shooter.runKicker(false); - }, shooter), - Commands.run(() -> shooter.setShooterSpeed(ShooterConstants.SHOOTER_SPEED), shooter) - .until(() -> shooter.getShooterVelocityRpm() >= ShooterConstants.SHOOTER_READY_RPM) - .withTimeout(1.0), - - // Keep intake running while the intake arm cycles up/down during shooting. - Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), - + // Turn 13 degrees right & bring shooter hood to HIGH Commands.parallel( - // Start kicker first, then start indexer 1 second later (kicker keeps running). - Commands.sequence( - Commands.run(() -> { - shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); - shooter.setIndexerSpeed(0.0); - }, shooter).withTimeout(1.0), - Commands.run(() -> { - shooter.setKickerSpeed(ShooterConstants.KICKER_SPEED); - shooter.setIndexerSpeed(ShooterConstants.INDEXER_SPEED); - //shooter.setHopperSpeed(ShooterConstants.HOPPER_SPEED); - }, shooter) - ), - - Commands.waitSeconds(2), - - // While shooting/indexing, continuously move the intake arm up/down. - Commands.sequence( - Commands.runOnce(intake::lowerIntake, intake), - Commands.waitUntil(() -> - Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_LOWERED_POSITION) - <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG), - Commands.runOnce(intake::raiseIntake, intake), - Commands.waitUntil(() -> - Math.abs(intake.getArmPositionDeg() - IntakeConstants.INTAKE_ARM_RAISED_POSITION) - <= IntakeConstants.INTAKE_ARM_TOLERANCE_DEG) - ) - .repeatedly()) - + turnRelativeDegrees(drive,-13.0), + Commands.runOnce(() -> shooter.setHoodAngle(ShooterSubsystem.HoodAngle.HIGH), shooter) + ) ); } From 8524c5877ddeac1ce4e6faa451c7d1740d8aca15 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Fri, 15 May 2026 13:23:07 -0500 Subject: [PATCH 93/96] lower turn-timeout --- src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java index 891e129..c0807b4 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java @@ -24,7 +24,7 @@ public class RightNeutralZoneAuto2 extends SequentialCommandGroup { private static final double DRIVE_HEADING_MAX_OMEGA_RAD_PER_SEC = 2.0; private static final double TURN_P = 4.0; private static final double TURN_TOLERANCE_DEG = 3.0; - private static final double TURN_TIMEOUT_SEC = 2.5; + private static final double TURN_TIMEOUT_SEC = 1.0; private static final double BACKWARD_METERS_1 = 3.6; private static final double BACKWARD_METERS_2 = 4.3; From 851ad6d9ea45364484495a082368ee758d63f942 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Fri, 15 May 2026 22:38:09 -0500 Subject: [PATCH 94/96] distance --- src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java index c0807b4..9f43883 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java @@ -28,6 +28,7 @@ public class RightNeutralZoneAuto2 extends SequentialCommandGroup { private static final double BACKWARD_METERS_1 = 3.6; private static final double BACKWARD_METERS_2 = 4.3; + private static final double BACKWARD_METERS_3 = 3.2; private static final double FORWARD_METERS_1 = 3.0; private static final double FORWARD_METERS_2 = 4.0; @@ -46,12 +47,12 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo // Turn 90 degrees right. turnRelativeDegrees(drive, -90.0), - // Drive forward 2m while starting intake (intake stays on for the rest of auto). + // Drive forward 3m while starting intake (intake stays on for the rest of auto). Commands.runOnce(() -> intake.setIntakePower(INTAKE_POWER), intake), driveDistanceMeters(drive, FORWARD_METERS_1, DRIVE_SPEED_MPS), - // Drive back 2m (intake still on). - driveDistanceMeters(drive, -FORWARD_METERS_1, DRIVE_SPEED_MPS), + // Drive back 3.2m (intake still on). + driveDistanceMeters(drive, -BACKWARD_METERS_3, DRIVE_SPEED_MPS), // Turn 90 degrees left (intake still on). turnRelativeDegrees(drive, 90.0), From e4ff477a74ab4275e9af0a0e2f84342d575be0c0 Mon Sep 17 00:00:00 2001 From: Lilly <113945482+raininglilly@users.noreply.github.com> Date: Sat, 16 May 2026 20:29:31 -0500 Subject: [PATCH 95/96] state --- .../java/frc/robot/Auto/RightNeutralZoneAuto2.java | 12 +++++++----- src/main/java/frc/robot/Constants.java | 2 +- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java index 9f43883..69a04e7 100644 --- a/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java +++ b/src/main/java/frc/robot/Auto/RightNeutralZoneAuto2.java @@ -28,17 +28,19 @@ public class RightNeutralZoneAuto2 extends SequentialCommandGroup { private static final double BACKWARD_METERS_1 = 3.6; private static final double BACKWARD_METERS_2 = 4.3; - private static final double BACKWARD_METERS_3 = 3.2; + private static final double BACKWARD_METERS_3 = 3.1; private static final double FORWARD_METERS_1 = 3.0; private static final double FORWARD_METERS_2 = 4.0; private static final double INTAKE_POWER = -0.75; - private static final double FEED_DURATION_SEC = 13.0; + private static final double FEED_DURATION_SEC = 10.0; public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, ShooterSubsystem shooter) { addRequirements(drive, intake, shooter); addCommands( + //Commands.waitSeconds(5.0), //GINGER WAS HERE + Commands.runOnce(intake::lowerIntake, intake), // Drive backwards 3.6m. @@ -57,13 +59,13 @@ public RightNeutralZoneAuto2(SwerveSubsystem drive, IntakeSubsystem intake, Shoo // Turn 90 degrees left (intake still on). turnRelativeDegrees(drive, 90.0), + // Drive forward (back to the trench) + driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), + // Stop intake at the end. Commands.runOnce(() -> intake.setIntakePower(0.0), intake), Commands.runOnce(() -> drive.drive(0, 0, 0, false), drive), - // Drive forward (back to the trench) - driveDistanceMeters(drive, BACKWARD_METERS_2, DRIVE_SPEED_MPS), - // Turn 13 degrees right & bring shooter hood to HIGH Commands.parallel( turnRelativeDegrees(drive,-13.0), diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e0f8e18..a12ec9c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -319,7 +319,7 @@ public static final class IntakeConstants { // These were previously colliding with ShooterConstants IDs (60/62) and causing robot init to crash. public static int INTAKE_ID = 19; // SparkMax.set(...) expects [-1.0, 1.0] percent output. - public static double INTAKE_SPEED = 0.75; // max percent output for intake motor + public static double INTAKE_SPEED = 0.80; // max percent output for intake motor public static int INTAKE_ARM_ID = 18; public static int INTAKE_ARM_2_ID = 24; From 5b9308dc5231f50d12250a1b0ff1336823328788 Mon Sep 17 00:00:00 2001 From: Seqi Date: Fri, 22 May 2026 16:28:30 -0500 Subject: [PATCH 96/96] Updated comments and cleaned unused imports --- .../frc/robot/Command/SimpleAutoAlign.java | 15 ++++++++------- src/main/java/frc/robot/Constants.java | 1 - src/main/java/frc/robot/RobotContainer.java | 19 +++++++------------ .../frc/robot/Subsystems/IntakeSubsystem.java | 1 - .../java/frc/robot/Tuning/TuningHelper.java | 2 -- 5 files changed, 15 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/Command/SimpleAutoAlign.java b/src/main/java/frc/robot/Command/SimpleAutoAlign.java index 5007bc0..f5b6108 100644 --- a/src/main/java/frc/robot/Command/SimpleAutoAlign.java +++ b/src/main/java/frc/robot/Command/SimpleAutoAlign.java @@ -31,7 +31,7 @@ public class SimpleAutoAlign extends Command { public static final double ROTATION_KI = 0; public static final double ROTATION_KD = 0; - //Tolerance and + //Tolerance and things public static final double DISTANCE_TOLERANCE_METERS = 0.08; public static final double ROTATION_TOLERANCE_DEG = 1.5; public static final double MAX_FORWARD_SPEED_MPS = 0; @@ -43,6 +43,7 @@ public class SimpleAutoAlign extends Command { public static final double DISTANCE_FILTER_ALPHA = 0.25; public static final double MAX_DISTANCE_ACCEL_MPS_PER_SEC = SwerveConstants.maxSpeed * 4.0; + //PID Controller and alignment private final PIDController distanceController = new PIDController(DISTANCE_KP, DISTANCE_KI, DISTANCE_KD); private final PIDController rotationController = new PIDController(ROTATION_KP, ROTATION_KI, ROTATION_KD); private int settledCycles = 0; @@ -57,20 +58,20 @@ public SimpleAutoAlign(SwerveSubsystem swerveSubsystem){ addRequirements(swerveSubsystem); } - private boolean canSeeTag() { + private boolean canSeeTag() { //checks if limelight can see tag double tv = NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("tv").getDouble(0.0); return tv > 0; } - private int getTagId() { + private int getTagId() { //gets the tag id from network tables return (int) NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("tid").getDouble(0.0); } - private boolean isSupportedTag(int tagId) { + private boolean isSupportedTag(int tagId) { //checks if we want to align to that tag return tagId == 10 || tagId == 11 ||tagId == 26 || tagId == 27 || tagId == 8 || tagId == 24; } - private double getDesiredAlignmentAngle(int tagId) { + private double getDesiredAlignmentAngle(int tagId) { //for each tag decides what angle the robot should be at in comparison to the tag if (tagId == 11 || tagId == 27) { return 20.0; } @@ -87,7 +88,7 @@ private double getOffsetToTarget() { return NetworkTableInstance.getDefault().getTable("limelight-a").getEntry("tx").getDouble(0.0); } - private double getDistanceToTargetMeters(double tyDegrees){ + private double getDistanceToTargetMeters(double tyDegrees){ //uses trig to find distance double angleToTargetDegrees = CAMERA_TILT_DEG + tyDegrees; if (Math.abs(angleToTargetDegrees) limelightCameras = new java.util.HashMap<>(); private UsbCamera driverCamera; @@ -202,11 +197,11 @@ private void configureBindings() { // SwerveSubsystem - The drive subsystem to control m_drive, // translationSupplier - Forward/backward speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(translationAxis) * 1.0, + () -> -getSpeedMultiplier() * driveController.getRawAxis(TRANSLATION_AXIS) * 1.0, // strafeSupplier - Side-to-side speed - () -> -getSpeedMultiplier() * driveController.getRawAxis(strafeAxis) * 1.0, + () -> -getSpeedMultiplier() * driveController.getRawAxis(STRAFE_AXIS) * 1.0, // rotationSupplier - Rotation speed - () -> -driveController.getRawAxis(rotationAxis) * 0.5, + () -> -driveController.getRawAxis(ROTATION_AXIS) * 0.5, // robotCentricSupplier - Robot-oriented (true) vs field-oriented (false) () -> robotCentric.getAsBoolean(), // isAutoAlignSupplier - Auto-align active flag diff --git a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java index c6ddaa1..351a044 100644 --- a/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/Subsystems/IntakeSubsystem.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.IntakeConstants; -import frc.robot.Tuning.TuningHelper; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; diff --git a/src/main/java/frc/robot/Tuning/TuningHelper.java b/src/main/java/frc/robot/Tuning/TuningHelper.java index 0b9ea71..45a0f09 100644 --- a/src/main/java/frc/robot/Tuning/TuningHelper.java +++ b/src/main/java/frc/robot/Tuning/TuningHelper.java @@ -1,7 +1,5 @@ package frc.robot.Tuning; -import java.time.Period; - import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class TuningHelper {