From c68329c9cac4515e1f14398d9ddab5a08b903db5 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Sun, 18 Jan 2026 12:53:07 -0700 Subject: [PATCH 01/29] Add aim lead --- .../java/frc4388/robot/RobotContainer.java | 8 ++--- .../frc4388/robot/constants/Constants.java | 13 -------- .../robot/constants/FieldConstants.java | 14 ++++++++ .../robot/subsystems/swerve/SwerveDrive.java | 33 ++++++++++++++----- .../swerve/SwerveDriveConstants.java | 12 +++++++ .../robot/subsystems/vision/VisionReal.java | 2 +- 6 files changed, 55 insertions(+), 27 deletions(-) create mode 100644 src/main/java/frc4388/robot/constants/FieldConstants.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5e81e40..ded3270 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -39,6 +39,7 @@ import frc4388.robot.commands.MoveUntilSuply; // import frc4388.robot.commands.wait.waitFeedCoral; import frc4388.robot.commands.wait.waitSupplier; import frc4388.robot.constants.Constants; +import frc4388.robot.constants.FieldConstants; import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; @@ -111,12 +112,9 @@ public class RobotContainer { // IF the driver is holding the aim button, aim the robot towards the hub if(m_driverXbox.getRightTriggerAxis() > 0.5) { // Aim - Translation2d shootTarget = new Translation2d(); - // new Rotation2() - Rotation2d ang = m_robotSwerveDrive.getPose2d().getTranslation().minus(shootTarget).getAngle(); - m_robotSwerveDrive.driveFieldAngle( + m_robotSwerveDrive.driveFacingPosition( getDeadbandedDriverController().getLeft(), - ang); + FieldConstants.BLUE_HUB_POS); } else { // Drive normally m_robotSwerveDrive.driveWithInput( diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index f9c9594..dd7b8af 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -92,19 +92,6 @@ public final class Constants { public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1); } - public static final class FieldConstants { - public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); - - // Test april tag field layout - // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( - // Arrays.asList(new AprilTag[] { - // new AprilTag(1, new Pose3d( - // new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) - // )), - // }), 100, 100); - - } - public static final class LEDConstants { public static final int LED_SPARK_ID = 9; diff --git a/src/main/java/frc4388/robot/constants/FieldConstants.java b/src/main/java/frc4388/robot/constants/FieldConstants.java new file mode 100644 index 0000000..ab79e79 --- /dev/null +++ b/src/main/java/frc4388/robot/constants/FieldConstants.java @@ -0,0 +1,14 @@ +package frc4388.robot.constants; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Translation2d; + +public final class FieldConstants { + public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + + public static final Translation2d BLUE_HUB_POS = new Translation2d(); + public static final Translation2d RED_HUB_POS = new Translation2d(); + + +} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index a927fe6..3ec59a0 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -251,14 +251,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable { io.setControl(ctrl); } + // Drive with a specific velocity and heading public void driveFieldAngle(Translation2d leftStick, Rotation2d heading) { if (leftStick.getNorm() < 0.05) // if no imput and the swerve drive is still going: stopModules(); // stop the swerve - // if (leftStick.getNorm() < 0.05) // if no imput - // return; // don't bother doing swerve drive math and return early. - - leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); @@ -268,13 +265,33 @@ public class SwerveDrive extends SubsystemBase implements Queryable { .withVelocityY(leftStick.getY() * speedAdjust) .withTargetDirection(heading); ctrl.HeadingController.setPID( - SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP, - SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI, - SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD + SwerveDriveConstants.PIDConstants.AIM_kP.get(), + SwerveDriveConstants.PIDConstants.AIM_kI.get(), + SwerveDriveConstants.PIDConstants.AIM_kD.get() + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kP, + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kI, + // SwerveDriveConstants.PIDConstants.AIM_GAINS.kD ); io.setControl(ctrl); // SmartDashboard.putBoolean("drift correction", true); - + } + + // Drive with the robot facing towards a specific position + public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) { + + // Get the current speed of the robot + Translation2d robotSpeed = new Translation2d( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond + ); + + // Calculate a point to aim ahead of the actual position. + Translation2d fieldPosLead = robotSpeed.times(SwerveDriveConstants.AIM_LEAD_TIME.get()).plus(fieldPos); + + // Calculate the angle between the current position and the lead position + Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); + + driveFieldAngle(leftStick, ang); } public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) { diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index f3bbe9f..acdf666 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -22,6 +22,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Angle; +import frc4388.utility.configurable.ConfigurableDouble; //import edu.wpi.first.units.measure.measure.Distance; import frc4388.utility.status.CanDevice; import frc4388.utility.structs.Gains; @@ -37,6 +38,9 @@ public final class SwerveDriveConstants { public static double PLAYBACK_ROTATION_SPEED = AUTO_MAX_ROT_SPEED; public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED; + // TODO: Replace with a constant + public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0); + public static final double CORRECTION_MIN = 10; public static final double CORRECTION_MAX = 50; @@ -156,6 +160,14 @@ public final class SwerveDriveConstants { public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(10, 0, 1); + + + + // TODO: Replace this with a static constant + public static final ConfigurableDouble AIM_kP = new ConfigurableDouble("Aim kP", 2.5); + public static final ConfigurableDouble AIM_kI = new ConfigurableDouble("Aim kI", 0); + public static final ConfigurableDouble AIM_kD = new ConfigurableDouble("Aim kD", 0.1); + // public static final Gains AIM_GAINS = new Gains(2.5, 0, 0.1); } public static final class Configurations { diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java index 1c12437..82fbd1f 100644 --- a/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java @@ -8,7 +8,7 @@ import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonPipelineResult; -import frc4388.robot.constants.Constants.FieldConstants; +import frc4388.robot.constants.FieldConstants; import frc4388.robot.constants.Constants.VisionConstants; public class VisionReal implements VisionIO { From f600317435c776b362c861c7ff7a4e0d40d41d2b Mon Sep 17 00:00:00 2001 From: Shikhar Date: Mon, 19 Jan 2026 13:42:16 -0700 Subject: [PATCH 02/29] Rotate and Autos --- simgui-ds.json | 90 +++++++++ .../deploy/pathplanner/autos/New Auto.auto | 19 ++ .../{Example Path.path => TurnNinety.path} | 10 +- .../java/frc4388/robot/RobotContainer.java | 42 ++--- src/main/java/frc4388/robot/RobotMap.java | 22 +-- .../commands/Autos/neoPlaybackChooser.java | 171 +++++++++--------- .../robot/constants/BuildConstants.java | 12 +- .../robot/constants/FieldConstants.java | 15 +- .../robot/subsystems/swerve/SwerveDrive.java | 3 +- .../swerve/SwerveDriveConstants.java | 2 +- .../robot/subsystems/vision/Vision.java | 1 + .../robot/subsystems/vision/VisionReal.java | 4 +- vendordeps/PathplannerLib-2025.2.7.json | 38 ---- vendordeps/photonlib.json | 12 +- 14 files changed, 258 insertions(+), 183 deletions(-) create mode 100644 simgui-ds.json create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto rename src/main/deploy/pathplanner/paths/{Example Path.path => TurnNinety.path} (89%) delete mode 100644 vendordeps/PathplannerLib-2025.2.7.json diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..3407179 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,90 @@ +{ + "System Joysticks": { + "window": { + "enabled": false + } + }, + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ] + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ] + } + ] +} diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 0000000..bb1c386 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TurnNinety" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/TurnNinety.path similarity index 89% rename from src/main/deploy/pathplanner/paths/Example Path.path rename to src/main/deploy/pathplanner/paths/TurnNinety.path index 3f475e5..60e6791 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/TurnNinety.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.0, - "y": 6.0 + "x": 2.0, + "y": 7.0 }, "prevControl": { - "x": 3.0, - "y": 6.0 + "x": 1.0, + "y": 7.0 }, "nextControl": null, "isLocked": false, @@ -48,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": -90.48219994972354 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ded3270..c72d1e6 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -9,47 +9,37 @@ package frc4388.robot; import java.io.File; -import edu.wpi.first.math.geometry.Rotation2d; +import com.pathplanner.lib.commands.PathPlannerAuto; + import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import frc4388.utility.DeferredBlock; -import frc4388.utility.compute.TimesNegativeOne; -import frc4388.utility.controller.ButtonBox; -import frc4388.utility.controller.DeadbandedXboxController; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.button.Trigger; - +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // Commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -// Autos -import frc4388.utility.controller.VirtualController; -import frc4388.utility.controller.XboxController; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc4388.robot.commands.MoveForTimeCommand; -import frc4388.robot.commands.alignment.RotTo45; -import frc4388.robot.commands.MoveUntilSuply; -// import frc4388.robot.commands.alignment.DriveToReef; -// import frc4388.robot.commands.wait.waitElevatorRefrence; -// import frc4388.robot.commands.wait.waitEndefectorRefrence; -// import frc4388.robot.commands.wait.waitFeedCoral; -import frc4388.robot.commands.wait.waitSupplier; +import frc4388.robot.commands.Autos.neoPlaybackChooser; import frc4388.robot.constants.Constants; -import frc4388.robot.constants.FieldConstants; -import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; - -import com.pathplanner.lib.commands.PathPlannerAuto; - +import frc4388.robot.constants.FieldConstants; // Subsystems import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; +import frc4388.utility.DeferredBlock; +import frc4388.utility.compute.TimesNegativeOne; +import frc4388.utility.controller.DeadbandedXboxController; +// Autos +import frc4388.utility.controller.VirtualController; +import frc4388.utility.controller.XboxController; /** * This class is where the bulk of the robot should be declared. Since @@ -65,7 +55,7 @@ public class RobotContainer { /* Subsystems */ public final LED m_robotLED = new LED(); - public final Vision m_vision = new Vision(); + public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera); // public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -91,7 +81,7 @@ public class RobotContainer { // ! /* Autos */ private SendableChooser autoChooser; private Command autoCommand; - + private neoPlaybackChooser m_autoChooser= new neoPlaybackChooser(m_robotSwerveDrive, null); public RobotContainer() { @@ -224,7 +214,7 @@ public class RobotContainer { } System.out.println("Robot Auto Changed " + filename); }); - // SmartDashboard.putData(autoChooser); + SmartDashboard.putData(autoChooser); } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 25f41d1..a7391b8 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -40,8 +40,8 @@ public class RobotMap { // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id); // public RobotGyro gyro = new RobotGyro(m_pigeon2); - // public final VisionIO leftCamera; - // public final VisionIO rightCamera; + public final VisionIO leftCamera; + public final VisionIO rightCamera; // public final LiDAR lidar = new @@ -61,15 +61,15 @@ public class RobotMap { public RobotMap(SimConstants.Mode mode) { switch (mode) { case REAL: - // // Configure cameras - // PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); - // PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); + // Configure cameras + PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); + PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); - // leftCamera = new VisionReal(leftCameraReal, VisionConstants.LEFT_CAMERA_POS); ; - // rightCamera = new VisionReal(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS); + leftCamera = new VisionReal(leftCameraReal, VisionConstants.RIGHT_CAMERA_POS); ; + rightCamera = new VisionReal(rightCameraReal, VisionConstants.LEFT_CAMERA_POS); - // FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera"); - // FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera"); + FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera"); + FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera"); // // Configure LiDAR // reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL); @@ -121,8 +121,8 @@ public class RobotMap { // case SIM: // break; default: - // leftCamera = new VisionIO() {}; - // rightCamera = new VisionIO() {}; + leftCamera = new VisionIO() {}; + rightCamera = new VisionIO() {}; // reefLidar = new LidarIO() {}; // reverseLidar = new LidarIO() {}; swerveDrivetrain = new SwerveIO() {}; diff --git a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java index f3d2987..c124e11 100644 --- a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java @@ -1,108 +1,107 @@ package frc4388.robot.commands.Autos; -// package frc4388.robot.commands.Autos; -// import java.io.File; -// import java.util.ArrayList; -// import java.util.HashMap; +import java.util.ArrayList; -// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -// import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; -// import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -// import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -// import edu.wpi.first.wpilibj2.command.Command; -// import edu.wpi.first.wpilibj2.command.InstantCommand; -// import frc4388.robot.commands.Swerve.JoystickPlayback; -// import frc4388.robot.commands.Swerve.neoJoystickPlayback; -// import frc4388.robot.subsystems.SwerveDrive; -// import frc4388.utility.controller.VirtualController; +import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import frc4388.robot.commands.Swerve.neoJoystickPlayback; +import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.utility.controller.VirtualController; -// public class neoPlaybackChooser { -// private final SendableChooser m_teamChosser = new SendableChooser(); -// private final SendableChooser m_possitionChosser = new SendableChooser(); -// private final SendableChooser m_autoNameChosser = new SendableChooser(); +public class neoPlaybackChooser { + private final SendableChooser m_teamChosser = new SendableChooser(); + private final SendableChooser m_possitionChosser = new SendableChooser(); + private final SendableChooser m_autoNameChosser = new SendableChooser(); -// private final SwerveDrive m_swerve; -// private final VirtualController[] m_controllers; -// // private final ArrayList> m_choosers = new ArrayList<>(); -// // private SendableChooser m_playback = null; -// private final ArrayList m_widgets = new ArrayList<>(); -// // private final HashMap m_commandPool = new HashMap<>(); + + private final SwerveDrive m_swerve; + private final VirtualController[] m_controllers; + // private final ArrayList> m_choosers = new ArrayList<>(); + // private SendableChooser m_playback = null; + private final ArrayList m_widgets = new ArrayList<>(); + // private final HashMap m_commandPool = new HashMap<>(); -// // private final File m_dir = new File("/home/lvuser/autos/"); -// // private int m_cmdNum = 0; + // private final File m_dir = new File("/home/lvuser/autos/"); + // private int m_cmdNum = 0; -// // commands -// private Command m_noAuto = new InstantCommand(); + // commands + private Command m_noAuto = new InstantCommand(); -// public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) { -// m_teamChosser.addOption("Red", "red"); -// m_teamChosser.setDefaultOption("Blue", "blue"); -// m_teamChosser.addOption("Nuetral", "nuetral"); -// m_possitionChosser.addOption("AMP", "amp"); -// m_possitionChosser.setDefaultOption("Center", "center"); -// m_possitionChosser.addOption("Source", "source"); -// m_swerve = swerve; -// m_controllers = controllers; -// } + public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) { + m_teamChosser.addOption("Red", "red"); + m_teamChosser.setDefaultOption("Blue", "blue"); + m_teamChosser.addOption("Nuetral", "nuetral"); + m_possitionChosser.addOption("AMP", "amp"); + m_possitionChosser.setDefaultOption("Center", "center"); + m_possitionChosser.addOption("Source", "source"); + m_swerve = swerve; + m_controllers = controllers; + SmartDashboard.putData("Team Chooser", m_teamChosser); + SmartDashboard.putData("Position Chooser", m_possitionChosser); + SmartDashboard.putData("Auto Name Chooser", m_autoNameChosser); + } -// public neoPlaybackChooser addOption(String name, String option) { -// m_autoNameChosser.addOption(name, option); -// return this; -// } + public neoPlaybackChooser addOption(String name, String option) { + m_autoNameChosser.addOption(name, option); + return this; + } -// // public PlaybackChooser buildDisplay() { -// // for (int i = 0; i < 10; i++) { -// // appendCommand(); -// // } -// // m_playback = m_choosers.get(0); -// // nextChooser(); + // public PlaybackChooser buildDisplay() { + // for (int i = 0; i < 10; i++) { + // appendCommand(); + // } + // m_playback = m_choosers.get(0); + // nextChooser(); -// // // ! This does not work, why? -// // Shuffleboard.getTab("Auto Chooser") -// // .add("Add Sequence", new InstantCommand(() -> nextChooser())) -// // .withPosition(4, 0); -// // return this; -// // } + // // ! This does not work, why? + // Shuffleboard.getTab("Auto Chooser") + // .add("Add Sequence", new InstantCommand(() -> nextChooser())) + // .withPosition(4, 0); + // return this; + // } -// // This will be bound to a button for the time being -// public void render() { -// // var chooser = new SendableChooser(); -// // // chooser.setDefaultOption("No Auto", m_noAuto); + // This will be bound to a button for the time being + // public void render() { + // // var chooser = new SendableChooser(); + // // // chooser.setDefaultOption("No Auto", m_noAuto); -// // m_choosers.add(chooser); -// ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser") -// .add("Command: " + m_choosers.size(), chooser) -// .withSize(4, 1) -// .withPosition(0, m_choosers.size() - 1) -// .withWidget(BuiltInWidgets.kSplitButtonChooser) -// .withWidget(BuiltInWidgets.kComboBoxChooser); + // // m_choosers.add(chooser); + // ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser") + // .add("Command: " + m_choosers.size(), chooser) + // .withSize(4, 1) + // .withPosition(0, m_choosers.size() - 1) + // .withWidget(BuiltInWidgets.kSplitButtonChooser) + // .withWidget(BuiltInWidgets.kComboBoxChooser); -// m_widgets.add(widget); -// } + // m_widgets.add(widget); + // } -// // public void nextChooser() { -// // SendableChooser chooser = m_choosers.get(m_cmdNum++); + // public void nextChooser() { + // SendableChooser chooser = m_choosers.get(m_cmdNum++); -// // String[] dirs = m_dir.list(); + // String[] dirs = m_dir.list(); -// // if(dirs != null){ // Fix funny error -// // for (String auto : dirs) { -// // chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); -// // } -// // } + // if(dirs != null){ // Fix funny error + // for (String auto : dirs) { + // chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); + // } + // } -// // for (var cmd_name : m_commandPool.keySet()) { -// // chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); -// // } -// // } + // for (var cmd_name : m_commandPool.keySet()) { + // chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); + // } + // } -// public String autoName() { -// return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto"; -// } + public String autoName() { + return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto"; + } -// public Command getCommand() { -// return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true); -// } -// } + public Command getCommand() { + return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true); + } +} diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 12dd413..8284d24 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 7; - public static final String GIT_SHA = "24fdd610c9256b4599b7d29ebb1a3acd14cc38b7"; - public static final String GIT_DATE = "2026-01-13 18:19:47 MST"; - public static final String GIT_BRANCH = "master"; - public static final String BUILD_DATE = "2026-01-13 18:58:23 MST"; - public static final long BUILD_UNIX_TIME = 1768355903336L; + public static final int GIT_REVISION = 9; + public static final String GIT_SHA = "c68329c9cac4515e1f14398d9ddab5a08b903db5"; + public static final String GIT_DATE = "2026-01-18 12:53:07 MST"; + public static final String GIT_BRANCH = "Autorotate"; + public static final String BUILD_DATE = "2026-01-19 13:36:57 MST"; + public static final long BUILD_UNIX_TIME = 1768855017879L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/constants/FieldConstants.java b/src/main/java/frc4388/robot/constants/FieldConstants.java index ab79e79..9fa9146 100644 --- a/src/main/java/frc4388/robot/constants/FieldConstants.java +++ b/src/main/java/frc4388/robot/constants/FieldConstants.java @@ -1,14 +1,27 @@ package frc4388.robot.constants; +import java.util.Arrays; + +import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; public final class FieldConstants { - public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + // public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); public static final Translation2d BLUE_HUB_POS = new Translation2d(); public static final Translation2d RED_HUB_POS = new Translation2d(); + // Test april tag field layout + public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( + Arrays.asList(new AprilTag[] { + new AprilTag(0, new Pose3d( + new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) + )), + }), 100, 100); } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 3ec59a0..6b5af3d 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -289,7 +289,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable { Translation2d fieldPosLead = robotSpeed.times(SwerveDriveConstants.AIM_LEAD_TIME.get()).plus(fieldPos); // Calculate the angle between the current position and the lead position - Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); + Rotation2d ang = fieldPosLead.minus(getPose2d().getTranslation()).getAngle(); + driveFieldAngle(leftStick, ang); } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index acdf666..9318444 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -164,7 +164,7 @@ public final class SwerveDriveConstants { // TODO: Replace this with a static constant - public static final ConfigurableDouble AIM_kP = new ConfigurableDouble("Aim kP", 2.5); + public static final ConfigurableDouble AIM_kP = new ConfigurableDouble("Aim kP", 15); public static final ConfigurableDouble AIM_kI = new ConfigurableDouble("Aim kI", 0); public static final ConfigurableDouble AIM_kD = new ConfigurableDouble("Aim kD", 0.1); // public static final Gains AIM_GAINS = new Gains(2.5, 0, 0.1); diff --git a/src/main/java/frc4388/robot/subsystems/vision/Vision.java b/src/main/java/frc4388/robot/subsystems/vision/Vision.java index a069af7..fedc22a 100644 --- a/src/main/java/frc4388/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/vision/Vision.java @@ -43,6 +43,7 @@ public class Vision extends SubsystemBase implements Queryable { io[i].updateInputs(state[i]); Logger.processInputs("Vision/Camera" + i , state[i]); } + Logger.recordOutput("Vision/isTagDectected", isTag()); } public List getPosesToAdd(){ diff --git a/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java index 82fbd1f..6f70542 100644 --- a/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java +++ b/src/main/java/frc4388/robot/subsystems/vision/VisionReal.java @@ -45,12 +45,12 @@ public class VisionReal implements VisionIO { var result = results.get(results.size()-1); - state.isTagDetected = state.isTagDetected | result.hasTargets(); - // If there are no tags if(!result.hasTargets()) return; + state.isTagDetected = state.isTagDetected | result.hasTargets(); + Optional estimatedRobotPose = getEstimatedGlobalPose(result, estimator); // If the tag was failed to be processed diff --git a/vendordeps/PathplannerLib-2025.2.7.json b/vendordeps/PathplannerLib-2025.2.7.json deleted file mode 100644 index da77d6b..0000000 --- a/vendordeps/PathplannerLib-2025.2.7.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "fileName": "PathplannerLib-2025.2.7.json", - "name": "PathplannerLib", - "version": "2025.2.7", - "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2026", - "mavenUrls": [ - "https://3015rangerrobotics.github.io/pathplannerlib/repo" - ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", - "javaDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-java", - "version": "2025.2.7" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-cpp", - "version": "2025.2.7", - "libName": "PathplannerLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 7b78472..9699738 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.0.1-beta", + "version": "v2025.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.0.1-beta", + "version": "v2025.3.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.0.1-beta", + "version": "v2025.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.0.1-beta", + "version": "v2025.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.0.1-beta" + "version": "v2025.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.0.1-beta" + "version": "v2025.3.1" } ] } \ No newline at end of file From d30c807b0735998c3181d2fa0c83fde4ab9b3a6c Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Mon, 19 Jan 2026 14:17:15 -0700 Subject: [PATCH 03/29] Autos --- .../java/frc4388/robot/RobotContainer.java | 3 +++ .../frc4388/robot/commands/Autos/Taxi.auto | 19 +++++++++++++++++++ 2 files changed, 22 insertions(+) create mode 100644 src/main/java/frc4388/robot/commands/Autos/Taxi.auto diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c72d1e6..5159c06 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -115,6 +115,9 @@ public class RobotContainer { }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); m_robotSwerveDrive.setToSlow(); + + makeAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); } diff --git a/src/main/java/frc4388/robot/commands/Autos/Taxi.auto b/src/main/java/frc4388/robot/commands/Autos/Taxi.auto new file mode 100644 index 0000000..a717089 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Autos/Taxi.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "taxi" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file From 7c8593a9f72fe5515886c9d6c122b14789652210 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Mon, 19 Jan 2026 15:21:40 -0700 Subject: [PATCH 04/29] Updates --- build.gradle | 2 +- .../java/frc4388/robot/RobotContainer.java | 4 +- .../frc4388/robot/commands/Autos/Taxi.auto | 19 ---- .../Autos/neo AutoRecoding format.txt | 20 ---- .../commands/Autos/neoPlaybackChooser.java | 107 ------------------ .../robot/constants/BuildConstants.java | 10 +- 6 files changed, 7 insertions(+), 155 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/Autos/Taxi.auto delete mode 100644 src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt delete mode 100644 src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java diff --git a/build.gradle b/build.gradle index 8862516..b7f2e02 100644 --- a/build.gradle +++ b/build.gradle @@ -124,4 +124,4 @@ gversion { dateFormat = "yyyy-MM-dd HH:mm:ss z" timeZone = "America/Denver" indent = " " -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5159c06..283b83a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -25,7 +25,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc4388.robot.commands.MoveForTimeCommand; -import frc4388.robot.commands.Autos.neoPlaybackChooser; import frc4388.robot.constants.Constants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; @@ -81,7 +80,6 @@ public class RobotContainer { // ! /* Autos */ private SendableChooser autoChooser; private Command autoCommand; - private neoPlaybackChooser m_autoChooser= new neoPlaybackChooser(m_robotSwerveDrive, null); public RobotContainer() { @@ -191,7 +189,7 @@ public class RobotContainer { dir = new File("/home/lvuser/deploy/pathplanner/autos/"); } else { // dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\"); - dir = new File("/home/astatin3/Documents/GitHub/2025RidgeScape/src/main/deploy/pathplanner/autos"); + dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2026KPopRobotHunters\\src\\main\\deploy\\pathplanner\\autos"); } String[] autos = dir.list(); diff --git a/src/main/java/frc4388/robot/commands/Autos/Taxi.auto b/src/main/java/frc4388/robot/commands/Autos/Taxi.auto deleted file mode 100644 index a717089..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/Taxi.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "taxi" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt b/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt deleted file mode 100644 index a65aea9..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/neo AutoRecoding format.txt +++ /dev/null @@ -1,20 +0,0 @@ -AUTO file format - -HEADER static size 0x5 -0x00 BYTE NUM AXES: defualts to 6 -0x01 BYTE NUM POV: defualts to 1 -0x02 BYTE NUM CONTROLLERS: defualts to 2 -0x03 SHORT FRAMES: any value greator or equal than one. - -FRAME PER CONTROLLER: defualt size 0x34 -0x00 DOUBLE AXES[NUM AXES] -0x30 SHORT BUTTONS -0x32 SHORT POVs[NUM POV] - -FRAME: size varrys -FRAME PER CONTROLLER[NUM CONTROLLERS] -INT UNIXTIMESTAMP - -FILE: -HEADER -FRAME[FRAMES] \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java deleted file mode 100644 index c124e11..0000000 --- a/src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java +++ /dev/null @@ -1,107 +0,0 @@ -package frc4388.robot.commands.Autos; - -import java.util.ArrayList; - -import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc4388.robot.commands.Swerve.neoJoystickPlayback; -import frc4388.robot.subsystems.swerve.SwerveDrive; -import frc4388.utility.controller.VirtualController; - -public class neoPlaybackChooser { - private final SendableChooser m_teamChosser = new SendableChooser(); - private final SendableChooser m_possitionChosser = new SendableChooser(); - private final SendableChooser m_autoNameChosser = new SendableChooser(); - - - private final SwerveDrive m_swerve; - private final VirtualController[] m_controllers; - // private final ArrayList> m_choosers = new ArrayList<>(); - // private SendableChooser m_playback = null; - private final ArrayList m_widgets = new ArrayList<>(); - // private final HashMap m_commandPool = new HashMap<>(); - - // private final File m_dir = new File("/home/lvuser/autos/"); - // private int m_cmdNum = 0; - - // commands - private Command m_noAuto = new InstantCommand(); - - public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) { - m_teamChosser.addOption("Red", "red"); - m_teamChosser.setDefaultOption("Blue", "blue"); - m_teamChosser.addOption("Nuetral", "nuetral"); - m_possitionChosser.addOption("AMP", "amp"); - m_possitionChosser.setDefaultOption("Center", "center"); - m_possitionChosser.addOption("Source", "source"); - m_swerve = swerve; - m_controllers = controllers; - SmartDashboard.putData("Team Chooser", m_teamChosser); - SmartDashboard.putData("Position Chooser", m_possitionChosser); - SmartDashboard.putData("Auto Name Chooser", m_autoNameChosser); - } - - public neoPlaybackChooser addOption(String name, String option) { - m_autoNameChosser.addOption(name, option); - return this; - } - - // public PlaybackChooser buildDisplay() { - // for (int i = 0; i < 10; i++) { - // appendCommand(); - // } - // m_playback = m_choosers.get(0); - // nextChooser(); - - // // ! This does not work, why? - // Shuffleboard.getTab("Auto Chooser") - // .add("Add Sequence", new InstantCommand(() -> nextChooser())) - // .withPosition(4, 0); - // return this; - // } - - // This will be bound to a button for the time being - // public void render() { - // // var chooser = new SendableChooser(); - // // // chooser.setDefaultOption("No Auto", m_noAuto); - - // // m_choosers.add(chooser); - // ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser") - // .add("Command: " + m_choosers.size(), chooser) - // .withSize(4, 1) - // .withPosition(0, m_choosers.size() - 1) - // .withWidget(BuiltInWidgets.kSplitButtonChooser) - // .withWidget(BuiltInWidgets.kComboBoxChooser); - - - // m_widgets.add(widget); - // } - - // public void nextChooser() { - // SendableChooser chooser = m_choosers.get(m_cmdNum++); - - // String[] dirs = m_dir.list(); - - // if(dirs != null){ // Fix funny error - // for (String auto : dirs) { - // chooser.addOption(auto, new JoystickPlayback(m_swerve, auto)); - // } - // } - - - // for (var cmd_name : m_commandPool.keySet()) { - // chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); - // } - // } - - public String autoName() { - return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto"; - } - - public Command getCommand() { - return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true); - } -} diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 8284d24..260cfa6 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 9; - public static final String GIT_SHA = "c68329c9cac4515e1f14398d9ddab5a08b903db5"; - public static final String GIT_DATE = "2026-01-18 12:53:07 MST"; + public static final int GIT_REVISION = 11; + public static final String GIT_SHA = "d30c807b0735998c3181d2fa0c83fde4ab9b3a6c"; + public static final String GIT_DATE = "2026-01-19 14:17:15 MST"; public static final String GIT_BRANCH = "Autorotate"; - public static final String BUILD_DATE = "2026-01-19 13:36:57 MST"; - public static final long BUILD_UNIX_TIME = 1768855017879L; + public static final String BUILD_DATE = "2026-01-19 15:17:53 MST"; + public static final long BUILD_UNIX_TIME = 1768861073727L; public static final int DIRTY = 1; private BuildConstants(){} From 11fa322f5922f7d432c0db927c77ec061c44eb81 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Mon, 19 Jan 2026 18:34:14 -0700 Subject: [PATCH 05/29] Autos Continued --- simgui-ds.json | 11 +++++++++-- .../pathplanner/autos/{New Auto.auto => Test.auto} | 0 .../java/frc4388/robot/constants/BuildConstants.java | 10 +++++----- 3 files changed, 14 insertions(+), 7 deletions(-) rename src/main/deploy/pathplanner/autos/{New Auto.auto => Test.auto} (100%) diff --git a/simgui-ds.json b/simgui-ds.json index 3407179..4a63cc1 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -62,7 +62,8 @@ 44, 46, 47 - ] + ], + "povCount": 0 }, { "axisConfig": [ @@ -84,7 +85,13 @@ 261, 269, 267 - ] + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 } ] } diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/Test.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/Test.auto diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 260cfa6..1221bcf 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 11; - public static final String GIT_SHA = "d30c807b0735998c3181d2fa0c83fde4ab9b3a6c"; - public static final String GIT_DATE = "2026-01-19 14:17:15 MST"; + public static final int GIT_REVISION = 12; + public static final String GIT_SHA = "7c8593a9f72fe5515886c9d6c122b14789652210"; + public static final String GIT_DATE = "2026-01-19 15:21:40 MST"; public static final String GIT_BRANCH = "Autorotate"; - public static final String BUILD_DATE = "2026-01-19 15:17:53 MST"; - public static final long BUILD_UNIX_TIME = 1768861073727L; + public static final String BUILD_DATE = "2026-01-19 18:33:26 MST"; + public static final long BUILD_UNIX_TIME = 1768872806551L; public static final int DIRTY = 1; private BuildConstants(){} From eb0d9de8d8342dc2b039ff2c80c2750776fa088f Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Mon, 19 Jan 2026 19:42:29 -0700 Subject: [PATCH 06/29] Finished autos --- .../pathplanner/autos/{Test.auto => Auto Test.auto} | 0 src/main/java/frc4388/robot/RobotContainer.java | 2 +- .../java/frc4388/robot/constants/BuildConstants.java | 10 +++++----- 3 files changed, 6 insertions(+), 6 deletions(-) rename src/main/deploy/pathplanner/autos/{Test.auto => Auto Test.auto} (100%) diff --git a/src/main/deploy/pathplanner/autos/Test.auto b/src/main/deploy/pathplanner/autos/Auto Test.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Test.auto rename to src/main/deploy/pathplanner/autos/Auto Test.auto diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 283b83a..85587d2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -189,7 +189,7 @@ public class RobotContainer { dir = new File("/home/lvuser/deploy/pathplanner/autos/"); } else { // dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2025RidgeScape\\src\\main\\deploy\\pathplanner\\autos\\"); - dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2026KPopRobotHunters\\src\\main\\deploy\\pathplanner\\autos"); + dir = new File("C:\\Users\\Ridgebotics\\Documents\\GitHub\\2026KPopRobotHunters\\src\\main\\deploy\\pathplanner\\autos\\"); } String[] autos = dir.list(); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 1221bcf..d38e8f9 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 12; - public static final String GIT_SHA = "7c8593a9f72fe5515886c9d6c122b14789652210"; - public static final String GIT_DATE = "2026-01-19 15:21:40 MST"; + public static final int GIT_REVISION = 13; + public static final String GIT_SHA = "11fa322f5922f7d432c0db927c77ec061c44eb81"; + public static final String GIT_DATE = "2026-01-19 18:34:14 MST"; public static final String GIT_BRANCH = "Autorotate"; - public static final String BUILD_DATE = "2026-01-19 18:33:26 MST"; - public static final long BUILD_UNIX_TIME = 1768872806551L; + public static final String BUILD_DATE = "2026-01-19 19:39:28 MST"; + public static final long BUILD_UNIX_TIME = 1768876768990L; public static final int DIRTY = 1; private BuildConstants(){} From ba16f53d6d8155249e7ca767b54fbfa1f17ce68e Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Tue, 20 Jan 2026 19:23:14 -0700 Subject: [PATCH 07/29] Autos work --- .../{Auto Test.auto => Auto Test Slow.auto} | 0 src/main/deploy/pathplanner/autos/Taxi.auto | 19 +++++++ src/main/deploy/pathplanner/paths/Taxi.path | 54 +++++++++++++++++++ .../deploy/pathplanner/paths/TurnNinety.path | 4 +- .../robot/constants/BuildConstants.java | 10 ++-- .../robot/subsystems/swerve/SwerveDrive.java | 23 +++++--- 6 files changed, 96 insertions(+), 14 deletions(-) rename src/main/deploy/pathplanner/autos/{Auto Test.auto => Auto Test Slow.auto} (100%) create mode 100644 src/main/deploy/pathplanner/autos/Taxi.auto create mode 100644 src/main/deploy/pathplanner/paths/Taxi.path diff --git a/src/main/deploy/pathplanner/autos/Auto Test.auto b/src/main/deploy/pathplanner/autos/Auto Test Slow.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Auto Test.auto rename to src/main/deploy/pathplanner/autos/Auto Test Slow.auto diff --git a/src/main/deploy/pathplanner/autos/Taxi.auto b/src/main/deploy/pathplanner/autos/Taxi.auto new file mode 100644 index 0000000..db06ab4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Taxi.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Taxi" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Taxi.path b/src/main/deploy/pathplanner/paths/Taxi.path new file mode 100644 index 0000000..4777d33 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Taxi.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9612559241706156, + "y": 7.0 + }, + "prevControl": { + "x": 1.9612559241706156, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.2, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TurnNinety.path b/src/main/deploy/pathplanner/paths/TurnNinety.path index 60e6791..e75bda6 100644 --- a/src/main/deploy/pathplanner/paths/TurnNinety.path +++ b/src/main/deploy/pathplanner/paths/TurnNinety.path @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 0.1, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": -90.48219994972354 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index d38e8f9..962dedb 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 13; - public static final String GIT_SHA = "11fa322f5922f7d432c0db927c77ec061c44eb81"; - public static final String GIT_DATE = "2026-01-19 18:34:14 MST"; + public static final int GIT_REVISION = 14; + public static final String GIT_SHA = "eb0d9de8d8342dc2b039ff2c80c2750776fa088f"; + public static final String GIT_DATE = "2026-01-19 19:42:29 MST"; public static final String GIT_BRANCH = "Autorotate"; - public static final String BUILD_DATE = "2026-01-19 19:39:28 MST"; - public static final long BUILD_UNIX_TIME = 1768876768990L; + public static final String BUILD_DATE = "2026-01-20 19:19:56 MST"; + public static final long BUILD_UNIX_TIME = 1768961996020L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 6b5af3d..bd1a358 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -10,6 +10,12 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.util.PathPlannerLogging; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -19,15 +25,9 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.compute.TimesNegativeOne; -import frc4388.utility.status.Status; import frc4388.utility.status.FaultReporter; import frc4388.utility.status.Queryable; - -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.util.PathPlannerLogging; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.RobotConfig; +import frc4388.utility.status.Status; public class SwerveDrive extends SubsystemBase implements Queryable { // private SwerveDrivetrain swerveDriveTrain; @@ -138,6 +138,15 @@ public class SwerveDrive extends SubsystemBase implements Queryable { initalPose2d = pose; io.resetPose(pose); } + // MIRA public void setOdoPose(Pose2d pose) { + // if (pose == null) return; + // io.tareEverything(); + // initalPose2d = pose; + // io.resetPose(pose); + // robotKnowsWhereItIs = true; + // rotTarget = pose.getRotation().getDegrees(); + // } + // public void oneModuleTest(SwerveModule module, Translation2d leftStick, // Translation2d rightStick){ From 7b71cfb9d2ec46fd4112f6e48e878d91e6cb1730 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Tue, 27 Jan 2026 16:46:12 -0700 Subject: [PATCH 08/29] intake start --- .../frc4388/robot/subsystems/TestRobot.java | 245 ++++++++++++++++++ .../robot/subsystems/intake/Intake.java | 58 +++++ .../subsystems/intake/IntakeConstants.java | 77 ++++++ .../robot/subsystems/intake/IntakeIO.java | 42 +++ .../robot/subsystems/intake/IntakeReal.java | 111 ++++++++ 5 files changed, 533 insertions(+) create mode 100644 src/main/java/frc4388/robot/subsystems/TestRobot.java create mode 100644 src/main/java/frc4388/robot/subsystems/intake/Intake.java create mode 100644 src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java create mode 100644 src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java create mode 100644 src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java diff --git a/src/main/java/frc4388/robot/subsystems/TestRobot.java b/src/main/java/frc4388/robot/subsystems/TestRobot.java new file mode 100644 index 0000000..87faa80 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/TestRobot.java @@ -0,0 +1,245 @@ +package frc4388.robot.subsystems; + +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.utility.configurable.ConfigurableDouble; + +public class TestRobot extends SubsystemBase { + + // TalonFX m_intakeMotor; + // TalonFX m_armMotor; + TalonFX m_intakeMotor; + TalonFX m_outerShooter; + TalonFX m_innerShooter; + + ConfigurableDouble intakeRate = new ConfigurableDouble("Intake Rate", 0); + + + ConfigurableDouble armUpPosition = new ConfigurableDouble("Arm Up Position", 0); + ConfigurableDouble armDownPosition = new ConfigurableDouble("Arm Down Position", 0); + + ConfigurableDouble storageRate = new ConfigurableDouble("Storage Rate", 0); + ConfigurableDouble outerRate = new ConfigurableDouble("Outer Rate", 0); + ConfigurableDouble innerRate = new ConfigurableDouble("Inner Rate", 0); + + VelocityDutyCycle outerVelocity = new VelocityDutyCycle(0); + VelocityDutyCycle innerVelocity = new VelocityDutyCycle(0); + + public static final double MAX_OUTER_VELOCITY = 1200; // Rots per second + + public enum RobotStare { + IntakeDown, + Idle, + Shoot + } + + public RobotStare robotStare = RobotStare.Idle; + + + public TestRobot( + // TalonFX intakeMotor, + // TalonFX armMotor, + TalonFX intakeMotor, + TalonFX outerShooter, + TalonFX innerShooter + ) { + // m_intakeMotor = intakeMotor; + // m_armMotor = armMotor; + m_intakeMotor = intakeMotor; + m_outerShooter = outerShooter; + m_innerShooter = innerShooter; + + // m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG); + // m_armMotor.getConfigurator().apply(ARM_MOTOR_CONFIG); + m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG); + m_outerShooter.getConfigurator().apply(OUTER_MOTOR_CONFIG); + m_innerShooter.getConfigurator().apply(INNER_MOTOR_CONFIG); + + m_outerShooter.getConfigurator().apply(SHOOTER_PID); + m_innerShooter.getConfigurator().apply(SHOOTER_PID); + + outerVelocity.Slot = 0; + innerVelocity.Slot = 0; + } + + // public static final TalonFXConfiguration INTAKE_MOTOR_CONFIG = new TalonFXConfiguration() + // .withCurrentLimits( + // new CurrentLimitsConfigs() + // .withStatorCurrentLimit(40) // TODO: tune??? + // .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means + // ).withMotorOutput( + // new MotorOutputConfigs() + // .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + // ); + // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() + // .withCurrentLimits( + // new CurrentLimitsConfigs() + // .withStatorCurrentLimit(40) // TODO: tune??? + // .withStatorCurrentLimitEnable(true) + // ).withMotorOutput( + // new MotorOutputConfigs() + // .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate + // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + // ); + + public static final TalonFXConfiguration INTAKE_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); + + public static final TalonFXConfiguration OUTER_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); + public static final TalonFXConfiguration INNER_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + + ); + + public static Slot0Configs SHOOTER_PID; + + static { + SHOOTER_PID = new Slot0Configs(); + SHOOTER_PID.kV = 0.12; + // SHOOTER_PID.kP = 0.11; + // SHOOTER_PID.kI = 0.48; + // SHOOTER_PID.kD = 0.01; + SHOOTER_PID.kP = 0.3; + SHOOTER_PID.kI = 0.0; + SHOOTER_PID.kD = 0.0; + } + + + ConfigurableDouble kV = new ConfigurableDouble("kV", 0.12); + ConfigurableDouble kP = new ConfigurableDouble("kP", 0.11); + ConfigurableDouble kI = new ConfigurableDouble("kI", 0.48); + ConfigurableDouble kD = new ConfigurableDouble("kD", 0.01); + + @Override + public void periodic() { + + SHOOTER_PID = new Slot0Configs(); + SHOOTER_PID.kV = kV.get(); + SHOOTER_PID.kP = kP.get(); + SHOOTER_PID.kI = kI.get(); + SHOOTER_PID.kD = kD.get(); + + m_outerShooter.getConfigurator().apply(SHOOTER_PID); + m_innerShooter.getConfigurator().apply(SHOOTER_PID); + + + SmartDashboard.putNumber("Outer Velocity", m_outerShooter.getVelocity().getValue().in(RotationsPerSecond)); + SmartDashboard.putNumber("Inner Velocity", m_innerShooter.getVelocity().getValue().in(RotationsPerSecond)); + + + switch (robotStare) { + case Idle: + + // Move the arm to the up position + PositionDutyCycle armPosition = new PositionDutyCycle(armUpPosition.get()); + // m_armMotor.setControl(armPosition); + + + // // Stop the intake motor + // m_intakeMotor.set(0); + + // // Stop the storage motor + // m_storageMotor.set(0); + + + // Stop the outer shooter motor + m_outerShooter.set(0); + // Stop the inner shooter motor + m_innerShooter.set(0); + + break; + case IntakeDown: + // Move the arm to the down sposition + PositionDutyCycle armPosition1 = new PositionDutyCycle(armDownPosition.get()); + // m_armMotor.setControl(armPosition1); + + // Move balls into the robot because the arm is down + VelocityDutyCycle intakeVelocity = new VelocityDutyCycle(intakeRate.get()); + // m_intakeMotor.setControl(intakeVelocity); + + // Move balls into the main box + VelocityDutyCycle storageVelocity = new VelocityDutyCycle(storageRate.get()); + // m_storageMotor.setControl(storageVelocity); + + + // Stop the outer shooter motor + m_outerShooter.set(0); + // Stop the inner shooter motor + m_innerShooter.set(0); + + break; + case Shoot: + + // Move the arm to the up position + PositionDutyCycle armPosition2 = new PositionDutyCycle(armUpPosition.get()); + // m_armMotor.setControl(armPosition2); + + // // Stop the intake motor + // m_intakeMotor.set(0); + + // // Move the balls into the + VelocityDutyCycle storageVelocity1 = new VelocityDutyCycle(-storageRate.get()); + // m_storageMotor.setControl(storageVelocity1); + + // outerVelocity. + // m_outerShooter.setControl(outerVelocity); + double outerRPM = outerRate.get(); + m_outerShooter.setControl(outerVelocity.withVelocity(outerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY)); + + + + // m_innerShooter.setControl(innerVelocity); + // m_innerShooter.set(innerRate.get()); + + double innerRPM = innerRate.get(); + m_innerShooter.setControl(innerVelocity.withVelocity(innerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY)); + + + // SmartDashboard.putNumber("Test", outerRate.get()); + + break; + default: + break; + } + + + + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..29e2ba2 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -0,0 +1,58 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.Rotation; + +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.utility.status.FaultReporter; + +public class Intake extends SubsystemBase { + IntakeIO io; + IntakeStateAutoLogged state = new IntakeStateAutoLogged(); + + Supplier m_swervePoseSupplier; + + public Intake( + IntakeIO io, + Supplier swervePoseSupplier + ) { + this.io = io; + this.m_swervePoseSupplier = swervePoseSupplier; + } + + public enum FieldZone { + // The robot should aim at the hub + InShootZone, + // The robot should aim towards the wall + AimAtWall, + + } + + // Calculate what should be done based off of the position of the robot + // TODO: Implement field zones + public FieldZone getTarget(Pose2d position) { + return FieldZone.InShootZone; + } + + @Override + public void periodic() { + + + + // FaultReporter.register(this); // TODO Implement fault reporter + + + Logger.processInputs("Intake", state); + + Pose2d pose = m_swervePoseSupplier.get(); + Angle robotRot = pose.getRotation().getMeasure(); + + io.updateInputs(state); + + } +} diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java new file mode 100644 index 0000000..f21fe8f --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -0,0 +1,77 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.units.measure.Angle; +import frc4388.utility.status.CanDevice; + +public class IntakeConstants { + // Motor conversions + // public static final double ANGLE_MOTOR_GEAR_RATIO = 1.; + public static final double PITCH_MOTOR_GEAR_RATIO = 1.; + public static final double FLYWHEEL_GEAR_RATIO = 1.; + public static final double FEEDER_INCHES_PER_ROT = 1.; + + // Limits + + // 0 is the forward angle on the robot. + // negative is left, positive is right + // public static final Angle ANGLE_LIMIT_LEFT = Degrees.of(-180); + // public static final Angle ANGLE_LIMIT_RIGHT = Degrees.of(180); + + // 0 is paralell to the ground, 90 is directly up + public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); + public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); + + // Motor configs + // public static final TalonFXConfiguration ANGLE_MOTOR_CONFIG = new TalonFXConfiguration() + // .withCurrentLimits( + // new CurrentLimitsConfigs() + // .withStatorCurrentLimit(40) // TODO: tune??? + // .withStatorCurrentLimitEnable(true) + // ).withMotorOutput( + // new MotorOutputConfigs() + // .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate + // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + // ); + + public static final class IDs { + public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22); + } + + public static final TalonFXConfiguration PITCH_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); + public static final TalonFXConfiguration FLYWHEEL_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); + public static final TalonFXConfiguration FEEDER_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); +} diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..35d82e1 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,42 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.units.CurrentUnit; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.LinearVelocity; + +public interface IntakeIO { + @AutoLog + public class IntakeState { + // Angle IntakeAngle = Rotations.of(0); + // Angle IntakeTargetAngle = Rotations.of(0); + // Current angleMotorCurrent = Amps.of(0); + + // Angle IntakePitch = Rotations.of(0); + // Angle IntakeTargetPitch = Rotations.of(0); + // Current pitchMotorCurrent = Amps.of(0); + + AngularVelocity flywheelVelocity = RotationsPerSecond.of(0); + AngularVelocity flywheelTargetVelocity = RotationsPerSecond.of(0); + Current flywheelMotorCurrent = Amps.of(0); + + LinearVelocity feederVelocity = InchesPerSecond.of(0); + LinearVelocity feederTargetVelocity = InchesPerSecond.of(0); + Current feederMotorCurrent = Amps.of(0); + } + + // public default void setIntakeAngle(IntakeState state, Angle angle) {} + // public default void setIntakePitch(IntakeState state, Angle angle) {} + public default void setFlywheelVelocity(IntakeState state, AngularVelocity angularVelocity) {} + public default void setFeederVelocity(IntakeState state, LinearVelocity linearVelocity) {} + + public default void updateInputs(IntakeState state) {} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java new file mode 100644 index 0000000..febf17a --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -0,0 +1,111 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.measure.*; + +public class IntakeReal implements IntakeIO { + + // TalonFX m_angleMotor; + // TalonFX m_pitchMotor; + TalonFX m_flywheelMotor; + TalonFX m_feederMotor; + + public IntakeReal( + // TalonFX angleMotor, + // TalonFX pitchMotor, + TalonFX flywheelMotor, + TalonFX feederMotor + ) { + // m_angleMotor = angleMotor; + // m_pitchMotor = pitchMotor; + m_flywheelMotor = flywheelMotor; + m_feederMotor = feederMotor; + + // Apply the configs + // m_angleMotor.getConfigurator().apply(IntakeConstants.ANGLE_MOTOR_CONFIG); + // m_pitchMotor.getConfigurator().apply(IntakeConstants.PITCH_MOTOR_CONFIG); + m_flywheelMotor.getConfigurator().apply(IntakeConstants.FLYWHEEL_MOTOR_CONFIG); + m_feederMotor.getConfigurator().apply(IntakeConstants.FEEDER_MOTOR_CONFIG); + } + + private Angle clampAng(Angle x, Angle min, Angle max){ + if(x.gt(max)) { + return max; + }else if(x.lt(min)) { + return min; + }else{ + return x; + } + } + + // // TODO: Test + // @Override + // public void setIntakeAngle(IntakeState state, Angle angle) { + // state.IntakeTargetAngle = angle; + // // Assume that the angle is always accurate, because I think we will use a shaft encoder + // // Assume that 0 degrees = forwards. Might need an offset here + + // Angle boundedAngle = clampAng(angle, IntakeConstants.ANGLE_LIMIT_LEFT, IntakeConstants.ANGLE_LIMIT_RIGHT); + // // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT + // double motorTargetAngle = boundedAngle.in(Rotations) / IntakeConstants.ANGLE_MOTOR_GEAR_RATIO; + // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); + // m_angleMotor.setControl(posRequest); + // } + + + // TODO: Test + // @Override + // public void setIntakePitch(IntakeState state, Angle angle) { + // state.IntakeTargetPitch = angle; + // // TODO: Test + // // This assumes that the 0 is paralell to the ground. Might need an offset here + + + // Angle boundedAngle = clampAng(angle, IntakeConstants.PITCH_LIMIT_UPPER, IntakeConstants.PITCH_LIMIT_LOWER); + // // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT + // double motorTargetAngle = boundedAngle.in(Rotations) / IntakeConstants.PITCH_MOTOR_GEAR_RATIO; + // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); + // m_pitchMotor.setControl(posRequest); + // } + + @Override + public void setFlywheelVelocity(IntakeState state, AngularVelocity angularVelocity) { + state.flywheelTargetVelocity = angularVelocity; + // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC) + double motorSpeed = angularVelocity.in(RotationsPerSecond) / IntakeConstants.FLYWHEEL_GEAR_RATIO; + VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed); + m_feederMotor.setControl(velocity); + } + + @Override + public void setFeederVelocity(IntakeState state, LinearVelocity linearVelocity) { + state.feederTargetVelocity = linearVelocity; + // (IN / SEC) * (ROT / IN) = (ROT / SEC) + double motorSpeed = linearVelocity.in(InchesPerSecond) / IntakeConstants.FEEDER_INCHES_PER_ROT; + VelocityDutyCycle velRequest = new VelocityDutyCycle(motorSpeed); + m_feederMotor.setControl(velRequest); + } + + @Override + public void updateInputs(IntakeState state) { + // state.IntakeAngle = m_angleMotor.getPosition().getValue().times(IntakeConstants.ANGLE_MOTOR_GEAR_RATIO); + // state.angleMotorCurrent = m_angleMotor.getStatorCurrent(false).getValue(); + + // state.IntakePitch = m_pitchMotor.getPosition().getValue().times(IntakeConstants.PITCH_MOTOR_GEAR_RATIO); + // state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue(); + + state.flywheelVelocity = m_flywheelMotor.getVelocity().getValue(); + state.flywheelMotorCurrent = m_flywheelMotor.getStatorCurrent().getValue(); + + state.feederVelocity = InchesPerSecond.of(m_feederMotor.getVelocity().getValue().in(RotationsPerSecond) * IntakeConstants.FEEDER_INCHES_PER_ROT); + state.feederMotorCurrent = m_feederMotor.getStatorCurrent().getValue(); + } + +} From 42e911fbdc7ad9af303bce96fb262829dfea7704 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Tue, 27 Jan 2026 18:13:51 -0700 Subject: [PATCH 09/29] Shoooterrr --- .../frc4388/robot/subsystems/TestRobot.java | 30 ++--- .../subsystems/shooter/ShooterConstants.java | 11 +- .../robot/subsystems/shooter/ShooterIO.java | 19 +-- .../robot/subsystems/shooter/ShooterReal.java | 120 +++++++++++------- 4 files changed, 110 insertions(+), 70 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/TestRobot.java b/src/main/java/frc4388/robot/subsystems/TestRobot.java index 87faa80..c435c43 100644 --- a/src/main/java/frc4388/robot/subsystems/TestRobot.java +++ b/src/main/java/frc4388/robot/subsystems/TestRobot.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.utility.configurable.ConfigurableDouble; @@ -19,7 +20,7 @@ public class TestRobot extends SubsystemBase { // TalonFX m_intakeMotor; // TalonFX m_armMotor; - TalonFX m_intakeMotor; + // TalonFX m_storageMotor; TalonFX m_outerShooter; TalonFX m_innerShooter; @@ -50,19 +51,19 @@ public class TestRobot extends SubsystemBase { public TestRobot( // TalonFX intakeMotor, // TalonFX armMotor, - TalonFX intakeMotor, + // TalonFX storageMotor, TalonFX outerShooter, TalonFX innerShooter ) { // m_intakeMotor = intakeMotor; // m_armMotor = armMotor; - m_intakeMotor = intakeMotor; + // m_storageMotor = storageMotor; m_outerShooter = outerShooter; m_innerShooter = innerShooter; // m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG); // m_armMotor.getConfigurator().apply(ARM_MOTOR_CONFIG); - m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG); + // m_storageMotor.getConfigurator().apply(STORAGE_MOTOR_CONFIG); m_outerShooter.getConfigurator().apply(OUTER_MOTOR_CONFIG); m_innerShooter.getConfigurator().apply(INNER_MOTOR_CONFIG); @@ -94,17 +95,16 @@ public class TestRobot extends SubsystemBase { // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means // ); - public static final TalonFXConfiguration INTAKE_MOTOR_CONFIG = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimit(40) // TODO: tune??? - .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means - ).withMotorOutput( - new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny - .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means - ); - + // public static final TalonFXConfiguration STORAGE_MOTOR_CONFIG = new TalonFXConfiguration() + // .withCurrentLimits( + // new CurrentLimitsConfigs() + // .withStatorCurrentLimit(40) // TODO: tune??? + // .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means + // ).withMotorOutput( + // new MotorOutputConfigs() + // .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + // ); public static final TalonFXConfiguration OUTER_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 59ce39b..857bbeb 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -1,6 +1,6 @@ package frc4388.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.Degrees; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; @@ -16,6 +16,9 @@ public class ShooterConstants { public static final double PITCH_MOTOR_GEAR_RATIO = 1.; public static final double FLYWHEEL_GEAR_RATIO = 1.; public static final double FEEDER_INCHES_PER_ROT = 1.; + public static final double SHOOTERMOTOR1_GEAR_RATIO = 1.; + public static final double SHOOTERMOTOR2_GEAR_RATIO = 1.; + public static final double INDEXER_GEAR_RATIO = 1.; // Limits @@ -44,7 +47,7 @@ public class ShooterConstants { public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22); } - public static final TalonFXConfiguration PITCH_MOTOR_CONFIG = new TalonFXConfiguration() + public static final TalonFXConfiguration SHOOTER1_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimit(40) // TODO: tune??? @@ -54,7 +57,7 @@ public class ShooterConstants { .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means ); - public static final TalonFXConfiguration FLYWHEEL_MOTOR_CONFIG = new TalonFXConfiguration() + public static final TalonFXConfiguration SHOOTER2_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimit(40) // TODO: tune??? @@ -64,7 +67,7 @@ public class ShooterConstants { .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means ); - public static final TalonFXConfiguration FEEDER_MOTOR_CONFIG = new TalonFXConfiguration() + public static final TalonFXConfiguration INDEXER_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() .withStatorCurrentLimit(40) // TODO: tune??? diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 999abc3..1ea9a10 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -24,19 +24,22 @@ public interface ShooterIO { // Angle shooterTargetPitch = Rotations.of(0); // Current pitchMotorCurrent = Amps.of(0); - AngularVelocity flywheelVelocity = RotationsPerSecond.of(0); - AngularVelocity flywheelTargetVelocity = RotationsPerSecond.of(0); - Current flywheelMotorCurrent = Amps.of(0); + AngularVelocity motor1TargetVelocity = RotationsPerSecond.of(0); + AngularVelocity motor2TargetVelocity = RotationsPerSecond.of(0); + AngularVelocity indexerTargetVelocity = RotationsPerSecond.of(0); + Current motor1Current = Amps.of(0); - LinearVelocity feederVelocity = InchesPerSecond.of(0); - LinearVelocity feederTargetVelocity = InchesPerSecond.of(0); - Current feederMotorCurrent = Amps.of(0); + LinearVelocity motorLinearVelocity = InchesPerSecond.of(0); + AngularVelocity motorVelocity = RotationsPerSecond.of(0); + AngularVelocity motorTargetVelocity = RotationsPerSecond.of(0); + Current motor2Current = Amps.of(0); } // public default void setShooterAngle(ShooterState state, Angle angle) {} // public default void setShooterPitch(ShooterState state, Angle angle) {} - public default void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {} - public default void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) {} + public default void setMotor1Velocity(ShooterState state, AngularVelocity angularVelocity) {} + public default void setMotor2Velocity(ShooterState state, AngularVelocity linearVelocity) {} + public default void setIndexerVelocity(ShooterState state, AngularVelocity linearVelocity) {} public default void updateInputs(ShooterState state) {} } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 9db0e8b..0f35c0c 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -1,38 +1,40 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.InchesPerSecond; -import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; -import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.units.measure.*; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.utility.configurable.ConfigurableDouble; -public class ShooterReal implements ShooterIO { +public class ShooterReal extends SubsystemBase implements ShooterIO { + + TalonFX m_shooter1Motor; + TalonFX m_shooter2Motor; + TalonFX m_indexerMotor; + VelocityDutyCycle shooter1Velocity = new VelocityDutyCycle(0); + VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0); + VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0); - // TalonFX m_angleMotor; - // TalonFX m_pitchMotor; - TalonFX m_flywheelMotor; - TalonFX m_feederMotor; public ShooterReal( - // TalonFX angleMotor, - // TalonFX pitchMotor, - TalonFX flywheelMotor, - TalonFX feederMotor + TalonFX shooter1Motor, + TalonFX shooter2Motor, + TalonFX indexerMotor ) { - // m_angleMotor = angleMotor; - // m_pitchMotor = pitchMotor; - m_flywheelMotor = flywheelMotor; - m_feederMotor = feederMotor; - - // Apply the configs - // m_angleMotor.getConfigurator().apply(ShooterConstants.ANGLE_MOTOR_CONFIG); - // m_pitchMotor.getConfigurator().apply(ShooterConstants.PITCH_MOTOR_CONFIG); - m_flywheelMotor.getConfigurator().apply(ShooterConstants.FLYWHEEL_MOTOR_CONFIG); - m_feederMotor.getConfigurator().apply(ShooterConstants.FEEDER_MOTOR_CONFIG); + m_shooter1Motor= shooter1Motor; + m_shooter2Motor= shooter2Motor; + m_indexerMotor = indexerMotor; + + + m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER1_MOTOR_CONFIG); + m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER2_MOTOR_CONFIG); + m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_MOTOR_CONFIG); } private Angle clampAng(Angle x, Angle min, Angle max){ @@ -45,6 +47,25 @@ public class ShooterReal implements ShooterIO { } } + public static Slot0Configs SHOOTER_PID; + + static { + SHOOTER_PID = new Slot0Configs(); + SHOOTER_PID.kV = 0.0; + SHOOTER_PID.kP = 0.0; + SHOOTER_PID.kI = 0.0; + SHOOTER_PID.kD = 0.0; + } + + + ConfigurableDouble kV = new ConfigurableDouble("Shooter kV", 0.12); + ConfigurableDouble kP = new ConfigurableDouble("Shooter kP", 0.11); + ConfigurableDouble kI = new ConfigurableDouble("Shooter kI", 0.48); + ConfigurableDouble kD = new ConfigurableDouble("Shooter kD", 0.01); + + + + // // TODO: Test // @Override // public void setShooterAngle(ShooterState state, Angle angle) { @@ -75,37 +96,50 @@ public class ShooterReal implements ShooterIO { // m_pitchMotor.setControl(posRequest); // } + @Override + public void periodic() { + + SHOOTER_PID = new Slot0Configs(); + SHOOTER_PID.kV = kV.get(); + SHOOTER_PID.kP = kP.get(); + SHOOTER_PID.kI = kI.get(); + SHOOTER_PID.kD = kD.get(); + + m_shooter1Motor.getConfigurator().apply(SHOOTER_PID); + m_shooter2Motor.getConfigurator().apply(SHOOTER_PID); + } + + @Override - public void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) { - state.flywheelTargetVelocity = angularVelocity; - // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC) - double motorSpeed = angularVelocity.in(RotationsPerSecond) / ShooterConstants.FLYWHEEL_GEAR_RATIO; - VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed); - m_feederMotor.setControl(velocity); + public void setMotor1Velocity(ShooterState state, AngularVelocity target) { + state.motor2TargetVelocity = target; + double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO; + m_indexerMotor.setControl(new VelocityDutyCycle(motorRps)); + } + + @Override + public void setMotor2Velocity(ShooterState state, AngularVelocity target) { + state.motor2TargetVelocity = target; + double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO; + m_indexerMotor.setControl(new VelocityDutyCycle(motorRps)); } @Override - public void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) { - state.feederTargetVelocity = linearVelocity; - // (IN / SEC) * (ROT / IN) = (ROT / SEC) - double motorSpeed = linearVelocity.in(InchesPerSecond) / ShooterConstants.FEEDER_INCHES_PER_ROT; - VelocityDutyCycle velRequest = new VelocityDutyCycle(motorSpeed); - m_feederMotor.setControl(velRequest); + public void setIndexerVelocity(ShooterState state, AngularVelocity target) { + state.indexerTargetVelocity = target; + double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO; + m_indexerMotor.setControl(new VelocityDutyCycle(motorRps)); } @Override public void updateInputs(ShooterState state) { - // state.shooterAngle = m_angleMotor.getPosition().getValue().times(ShooterConstants.ANGLE_MOTOR_GEAR_RATIO); - // state.angleMotorCurrent = m_angleMotor.getStatorCurrent(false).getValue(); - // state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO); - // state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue(); + state.motorVelocity = m_shooter1Motor.getVelocity().getValue(); + state.motorVelocity = m_shooter2Motor.getVelocity().getValue(); + state.motorVelocity = m_indexerMotor.getVelocity().getValue(); - state.flywheelVelocity = m_flywheelMotor.getVelocity().getValue(); - state.flywheelMotorCurrent = m_flywheelMotor.getStatorCurrent().getValue(); - - state.feederVelocity = InchesPerSecond.of(m_feederMotor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT); - state.feederMotorCurrent = m_feederMotor.getStatorCurrent().getValue(); + state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT); + state.motor1Current = m_shooter1Motor.getStatorCurrent().getValue(); } } From 4ef804c4ce047ccd17884518200bf2dcf036f65e Mon Sep 17 00:00:00 2001 From: Shikhar Date: Tue, 27 Jan 2026 19:15:37 -0700 Subject: [PATCH 10/29] Shooter finish --- .../subsystems/shooter/ShooterConstants.java | 11 +++- .../robot/subsystems/shooter/ShooterIO.java | 10 +++- .../robot/subsystems/shooter/ShooterReal.java | 58 +++++-------------- 3 files changed, 31 insertions(+), 48 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 857bbeb..e938be4 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -4,6 +4,7 @@ import static edu.wpi.first.units.Units.Degrees; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -12,14 +13,18 @@ import frc4388.utility.status.CanDevice; public class ShooterConstants { // Motor conversions - // public static final double ANGLE_MOTOR_GEAR_RATIO = 1.; - public static final double PITCH_MOTOR_GEAR_RATIO = 1.; - public static final double FLYWHEEL_GEAR_RATIO = 1.; + public static final double FEEDER_INCHES_PER_ROT = 1.; public static final double SHOOTERMOTOR1_GEAR_RATIO = 1.; public static final double SHOOTERMOTOR2_GEAR_RATIO = 1.; public static final double INDEXER_GEAR_RATIO = 1.; + public static Slot0Configs SHOOTER_PID = new Slot0Configs() + .withKV(0.0) + .withKP(0.0) + .withKI(0.0) + .withKD(0.0); + // Limits // 0 is the forward angle on the robot. diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 1ea9a10..5f0a1b8 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -27,12 +27,16 @@ public interface ShooterIO { AngularVelocity motor1TargetVelocity = RotationsPerSecond.of(0); AngularVelocity motor2TargetVelocity = RotationsPerSecond.of(0); AngularVelocity indexerTargetVelocity = RotationsPerSecond.of(0); + + AngularVelocity motor1Velocity = RotationsPerSecond.of(0); + AngularVelocity motor2Velocity = RotationsPerSecond.of(0); + AngularVelocity indexerVelocity = RotationsPerSecond.of(0); + Current motor1Current = Amps.of(0); + Current motor2Current = Amps.of(0); + Current indexerCurrent = Amps.of(0); LinearVelocity motorLinearVelocity = InchesPerSecond.of(0); - AngularVelocity motorVelocity = RotationsPerSecond.of(0); - AngularVelocity motorTargetVelocity = RotationsPerSecond.of(0); - Current motor2Current = Amps.of(0); } // public default void setShooterAngle(ShooterState state, Angle angle) {} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 0f35c0c..159c70b 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -12,7 +12,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.utility.configurable.ConfigurableDouble; -public class ShooterReal extends SubsystemBase implements ShooterIO { +public class ShooterReal implements ShooterIO { TalonFX m_shooter1Motor; TalonFX m_shooter2Motor; @@ -30,7 +30,10 @@ public class ShooterReal extends SubsystemBase implements ShooterIO { m_shooter1Motor= shooter1Motor; m_shooter2Motor= shooter2Motor; m_indexerMotor = indexerMotor; - + m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); + m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); + m_indexerMotor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); + m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER1_MOTOR_CONFIG); m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER2_MOTOR_CONFIG); @@ -47,24 +50,9 @@ public class ShooterReal extends SubsystemBase implements ShooterIO { } } - public static Slot0Configs SHOOTER_PID; - - static { - SHOOTER_PID = new Slot0Configs(); - SHOOTER_PID.kV = 0.0; - SHOOTER_PID.kP = 0.0; - SHOOTER_PID.kI = 0.0; - SHOOTER_PID.kD = 0.0; - } - ConfigurableDouble kV = new ConfigurableDouble("Shooter kV", 0.12); - ConfigurableDouble kP = new ConfigurableDouble("Shooter kP", 0.11); - ConfigurableDouble kI = new ConfigurableDouble("Shooter kI", 0.48); - ConfigurableDouble kD = new ConfigurableDouble("Shooter kD", 0.01); - - - + // // TODO: Test // @Override @@ -96,32 +84,15 @@ public class ShooterReal extends SubsystemBase implements ShooterIO { // m_pitchMotor.setControl(posRequest); // } - @Override - public void periodic() { - - SHOOTER_PID = new Slot0Configs(); - SHOOTER_PID.kV = kV.get(); - SHOOTER_PID.kP = kP.get(); - SHOOTER_PID.kI = kI.get(); - SHOOTER_PID.kD = kD.get(); - - m_shooter1Motor.getConfigurator().apply(SHOOTER_PID); - m_shooter2Motor.getConfigurator().apply(SHOOTER_PID); - } - @Override public void setMotor1Velocity(ShooterState state, AngularVelocity target) { + state.motor1TargetVelocity = target; state.motor2TargetVelocity = target; + double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO; - m_indexerMotor.setControl(new VelocityDutyCycle(motorRps)); - } - - @Override - public void setMotor2Velocity(ShooterState state, AngularVelocity target) { - state.motor2TargetVelocity = target; - double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO; - m_indexerMotor.setControl(new VelocityDutyCycle(motorRps)); + m_shooter1Motor.setControl(new VelocityDutyCycle(motorRps)); + m_shooter2Motor.setControl(new VelocityDutyCycle(motorRps)); } @Override @@ -134,12 +105,15 @@ public class ShooterReal extends SubsystemBase implements ShooterIO { @Override public void updateInputs(ShooterState state) { - state.motorVelocity = m_shooter1Motor.getVelocity().getValue(); - state.motorVelocity = m_shooter2Motor.getVelocity().getValue(); - state.motorVelocity = m_indexerMotor.getVelocity().getValue(); + state.motor1Velocity = m_shooter1Motor.getVelocity().getValue(); + state.motor2Velocity = m_shooter2Motor.getVelocity().getValue(); + state.indexerVelocity = m_indexerMotor.getVelocity().getValue(); state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT); + state.motor1Current = m_shooter1Motor.getStatorCurrent().getValue(); + state.motor2Current = m_shooter2Motor.getStatorCurrent().getValue(); + state.indexerCurrent = m_indexerMotor.getStatorCurrent().getValue(); } } From fa3c54b276f03f95b2096cc1702071647ceba92c Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Tue, 27 Jan 2026 18:16:23 -0800 Subject: [PATCH 11/29] Intake bp bp for intake with setmode method --- .../robot/constants/BuildConstants.java | 12 +-- .../robot/subsystems/intake/Intake.java | 78 ++++++++++++++++ .../subsystems/intake/IntakeConstants.java | 80 +++++++++++++++++ .../robot/subsystems/intake/IntakeIO.java | 42 +++++++++ .../robot/subsystems/intake/IntakeReal.java | 88 +++++++++++++++++++ 5 files changed, 294 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/intake/Intake.java create mode 100644 src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java create mode 100644 src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java create mode 100644 src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 962dedb..f7e73de 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 14; - public static final String GIT_SHA = "eb0d9de8d8342dc2b039ff2c80c2750776fa088f"; - public static final String GIT_DATE = "2026-01-19 19:42:29 MST"; - public static final String GIT_BRANCH = "Autorotate"; - public static final String BUILD_DATE = "2026-01-20 19:19:56 MST"; - public static final long BUILD_UNIX_TIME = 1768961996020L; + public static final int GIT_REVISION = 15; + public static final String GIT_SHA = "ba16f53d6d8155249e7ca767b54fbfa1f17ce68e"; + public static final String GIT_DATE = "2026-01-20 19:23:14 MST"; + public static final String GIT_BRANCH = "Intake-boilerplate"; + public static final String BUILD_DATE = "2026-01-27 18:56:30 MST"; + public static final long BUILD_UNIX_TIME = 1769565390510L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..1aac079 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -0,0 +1,78 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.Rotation; + +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.utility.status.FaultReporter; + +public class Intake extends SubsystemBase { + IntakeIO io; + IntakeStateAutoLogged state = new IntakeStateAutoLogged(); + + Supplier m_swervePoseSupplier; + + public Intake( + IntakeIO io, + Supplier swervePoseSupplier + ) { + this.io = io; + this.m_swervePoseSupplier = swervePoseSupplier; + } + + public enum IntakeMode { + Up, + Down, + } + + public void setMode(IntakeMode mode) { + switch (mode) { + case Up: + io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER); + break; + case Down: + io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER); + io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY); + break; + } + } + + + // public enum FieldZone { + // // The robot should aim at the hub + // InShootZone, + // // The robot should aim towards the wall + // AimAtWall, + + // } + + // // Calculate what should be done based off of the position of the robot + // // TODO: Implement field zones + // public FieldZone getTarget(Pose2d position) { + // return FieldZone.InShootZone; + // } + + @Override + public void periodic() { + + + + // FaultReporter.register(this); // TODO Implement fault reporter + + + Logger.processInputs("Intake", state); + + Pose2d pose = m_swervePoseSupplier.get(); + Angle robotRot = pose.getRotation().getMeasure(); + + io.updateInputs(state); + + } +} + + diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java new file mode 100644 index 0000000..2e3477d --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -0,0 +1,80 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import frc4388.utility.status.CanDevice; + +public class IntakeConstants { + // Motor conversions + + public static final double ARM_MOTOR_GEAR_RATIO = 1.; + public static final double ROLLER_MOTOR_GEAR_RATIO = 1.; + + // Limits + + // 0 is the forward angle on the robot. + // negative is left, positive is right + public static final Angle ARM_LIMIT_LOWER = Degrees.of(-180); + public static final Angle ARM_LIMIT_UPPER = Degrees.of(180); + public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(0.0); + + public static final Slot0Configs ARM_PID = new Slot0Configs() + .withKP(2.0) + .withKI(0.0) + .withKD(10.0); + + public static final Slot1Configs ROLLER_PID = new Slot1Configs() + .withKP(2.0) + .withKI(0.0) + .withKD(10.0); + + // 0 is paralell to the ground, 90 is directly up + // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); + // public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); + + // Motor configs + public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); + + public static final class IDs { + public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22); + } + // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() + // .withCurrentLimits( + // new CurrentLimitsConfigs() + // .withStatorCurrentLimit(40) // TODO: tune??? + // .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means + // ).withMotorOutput( + // new MotorOutputConfigs() + // .withNeutralMode(NeutralModeValue.Brake) // Brake so it stop + // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + // ); + public static final TalonFXConfiguration ROLLER_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); +} diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java new file mode 100644 index 0000000..92ba274 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -0,0 +1,42 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.units.CurrentUnit; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.LinearVelocity; + +public interface IntakeIO { + @AutoLog + public class IntakeState { + Angle armAngle = Rotations.of(0); + Angle armTargetAngle = Rotations.of(0); + Current armMotorCurrent = Amps.of(0); + + // Angle shooterPitch = Rotations.of(0); + // Angle shooterTargetPitch = Rotations.of(0); + // Current pitchMotorCurrent = Amps.of(0); + + AngularVelocity rollerVelocity = RotationsPerSecond.of(0); + AngularVelocity rollerTargetVelocity = RotationsPerSecond.of(0); + Current rollerMotorCurrent = Amps.of(0); + + // LinearVelocity feederVelocity = InchesPerSecond.of(0); + // LinearVelocity feederTargetVelocity = InchesPerSecond.of(0); + // Current feederMotorCurrent = Amps.of(0); + } + + // public default void setShooterAngle(ShooterState state, Angle angle) {} + // public default void setShooterPitch(ShooterState state, Angle angle) {} + public default void setArmAngle(IntakeState state, Angle angle) {} + public default void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {} + + public default void updateInputs(IntakeState state) {} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java new file mode 100644 index 0000000..1f8b5b3 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -0,0 +1,88 @@ +package frc4388.robot.subsystems.intake; + +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.measure.*; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.utility.configurable.ConfigurableDouble; + +public class IntakeReal implements IntakeIO { + + + TalonFX m_armMotor; + TalonFX m_rollerMotor; + + public IntakeReal( + + TalonFX armMotor, + TalonFX rollerMotor + ) { + // m_angleMotor = angleMotor; + // m_pitchMotor = pitchMotor; + m_armMotor = armMotor; + m_rollerMotor = rollerMotor; + + // Apply the configs + m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID); + m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG); + m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_PID); + m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG); + } + + private Angle clampAng(Angle x, Angle min, Angle max){ + if(x.gt(max)) { + return max; + }else if(x.lt(min)) { + return min; + }else{ + return x; + } + } + + + + @Override + public void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) { + state.rollerTargetVelocity = angularVelocity; + // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC) + double motorSpeed = angularVelocity.in(RotationsPerSecond) / IntakeConstants.ROLLER_MOTOR_GEAR_RATIO; + VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed); + m_rollerMotor.setControl(velocity); + } + + @Override + public void setArmAngle(IntakeState state, Angle angle) { + state.armTargetAngle = angle; + // Assume that the angle is always accurate, because I think we will use a shaft encoder + // Assume that 0 degrees = forwards. Might need an offset here + + Angle boundedAngle = clampAng(angle, IntakeConstants.ARM_LIMIT_LOWER, IntakeConstants.ARM_LIMIT_UPPER); + // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT + double motorTargetAngle = boundedAngle.in(Rotations) / IntakeConstants.ARM_MOTOR_GEAR_RATIO; + PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); + m_armMotor.setControl(posRequest); + } + + @Override + public void updateInputs(IntakeState state) { + state.armAngle = m_armMotor.getPosition().getValue().times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); + state.armMotorCurrent = m_armMotor.getStatorCurrent(false).getValue(); + + // state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO); + // state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue(); + + // state.armAngle = m_armMotor.getPosition().getValue(); + // state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue(); + + state.rollerVelocity = m_rollerMotor.getVelocity().getValue(); + state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue(); + } +} \ No newline at end of file From 8bda61b9837d7450a2fb4650bf02ffb1f4f06213 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Thu, 29 Jan 2026 16:59:53 -0700 Subject: [PATCH 12/29] RobotMap intake and shooter --- src/main/java/frc4388/robot/RobotMap.java | 36 +++++++++++++------ .../robot/constants/BuildConstants.java | 12 +++---- .../subsystems/intake/IntakeConstants.java | 7 ++++ .../subsystems/shooter/ShooterConstants.java | 7 ++-- 4 files changed, 42 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index a7391b8..f0a4400 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -20,6 +20,12 @@ import edu.wpi.first.wpilibj.DigitalInput; //import frc4388.robot.constants.Constants.ElevatorConstants; import frc4388.robot.constants.Constants.SimConstants; import frc4388.robot.constants.Constants.VisionConstants; +import frc4388.robot.subsystems.intake.IntakeConstants; +import frc4388.robot.subsystems.intake.IntakeIO; +import frc4388.robot.subsystems.intake.IntakeReal; +import frc4388.robot.subsystems.shooter.ShooterConstants; +import frc4388.robot.subsystems.shooter.ShooterIO; +import frc4388.robot.subsystems.shooter.ShooterReal; // import frc4388.robot.subsystems.elevator.ElevatorIO; // import frc4388.robot.subsystems.elevator.ElevatorReal; import frc4388.robot.subsystems.swerve.SwerveDriveConstants; @@ -56,7 +62,8 @@ public class RobotMap { public final SwerveIO swerveDrivetrain; // /* Elevator Subsystem */ - // public final ElevatorIO elevatorIO; + public final ShooterIO shooterIO; + public final IntakeIO intakeIO; public RobotMap(SimConstants.Mode mode) { switch (mode) { @@ -84,25 +91,33 @@ public class RobotMap { swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); - // Configure elevator + // Configure Shooter - // TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id); - // TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id); + TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id); + TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id); + TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id); + //Configure Intake + TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id); + TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id); // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - // elevatorIO = new ElevatorReal(elevator, endeffector, basinLimitSwitch, endeffectorLimitSwitch, IRIntakeBeam); - + shooterIO = new ShooterReal(shooter1, shooter2, indexer); + intakeIO = new IntakeReal(arm, roller); // Fault FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); - // FaultTalonFX.addDevice(elevator, "Elevator"); - // FaultTalonFX.addDevice(endeffector, "Endeffector"); + + FaultTalonFX.addDevice(shooter1, "Shooter1"); + FaultTalonFX.addDevice(shooter2, "Shooter2"); + FaultTalonFX.addDevice(indexer, "Indexer"); + FaultTalonFX.addDevice(arm, "Arm"); + FaultTalonFX.addDevice(roller, "Roller"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); @@ -123,10 +138,9 @@ public class RobotMap { default: leftCamera = new VisionIO() {}; rightCamera = new VisionIO() {}; - // reefLidar = new LidarIO() {}; - // reverseLidar = new LidarIO() {}; swerveDrivetrain = new SwerveIO() {}; - // elevatorIO = new ElevatorIO() {}; + intakeIO = new IntakeIO() {}; + shooterIO = new ShooterIO() {}; break; } } diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index f7e73de..a6c72ad 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 15; - public static final String GIT_SHA = "ba16f53d6d8155249e7ca767b54fbfa1f17ce68e"; - public static final String GIT_DATE = "2026-01-20 19:23:14 MST"; - public static final String GIT_BRANCH = "Intake-boilerplate"; - public static final String BUILD_DATE = "2026-01-27 18:56:30 MST"; - public static final long BUILD_UNIX_TIME = 1769565390510L; + public static final int GIT_REVISION = 22; + public static final String GIT_SHA = "e2797ab77871b91d546e6e9793852cf94d8b712f"; + public static final String GIT_DATE = "2026-01-27 19:28:57 MST"; + public static final String GIT_BRANCH = "Subsystem-Boilerplate"; + public static final String BUILD_DATE = "2026-01-29 16:59:21 MST"; + public static final long BUILD_UNIX_TIME = 1769731161482L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 2e3477d..48600d5 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -20,6 +20,13 @@ public class IntakeConstants { public static final double ARM_MOTOR_GEAR_RATIO = 1.; public static final double ROLLER_MOTOR_GEAR_RATIO = 1.; + + //IDs + + + public static final CanDevice ARM_ID = new CanDevice("SHOOTER 1", 20); + public static final CanDevice ROLLER_ID = new CanDevice("SHOOTER 2", 21); + // Limits // 0 is the forward angle on the robot. diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index e938be4..1e80500 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -48,9 +48,10 @@ public class ShooterConstants { // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means // ); - public static final class IDs { - public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22); - } + public static final CanDevice SHOOTER1_ID = new CanDevice("SHOOTER 1", 22); + public static final CanDevice SHOOTER2_ID = new CanDevice("SHOOTER 2", 23); + public static final CanDevice INDEXER_ID = new CanDevice("INDEXER",24); + public static final TalonFXConfiguration SHOOTER1_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( From 5d381731689602f5fac161f1552170003ef30c14 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Thu, 29 Jan 2026 18:07:19 -0700 Subject: [PATCH 13/29] Updates --- .../java/frc4388/robot/RobotContainer.java | 23 ++++++++++-- .../robot/constants/BuildConstants.java | 12 +++--- .../java/frc4388/robot/subsystems/LED.java | 2 +- .../robot/subsystems/intake/Intake.java | 1 + .../subsystems/intake/IntakeConstants.java | 4 +- .../robot/subsystems/shooter/Shooter.java | 37 +++++++++++++++++-- .../subsystems/shooter/ShooterConstants.java | 11 ++++++ 7 files changed, 76 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 85587d2..9d1cf19 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -19,18 +19,25 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // Commands import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc4388.robot.commands.MoveForTimeCommand; +import frc4388.robot.commands.MoveUntilSuply; import frc4388.robot.constants.Constants; +import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; import frc4388.robot.constants.FieldConstants; // Subsystems import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.shooter.Shooter; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; @@ -39,6 +46,7 @@ import frc4388.utility.controller.DeadbandedXboxController; // Autos import frc4388.utility.controller.VirtualController; import frc4388.utility.controller.XboxController; +import frc4388.utility.structs.LEDPatterns; /** * This class is where the bulk of the robot should be declared. Since @@ -55,8 +63,8 @@ public class RobotContainer { /* Subsystems */ public final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera); - // public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); + private Boolean operatorManualMode = false; // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); // public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef"); @@ -81,6 +89,10 @@ public class RobotContainer { private SendableChooser autoChooser; private Command autoCommand; + private Command RobotShoot = new SequentialCommandGroup( + new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.PARTY_TWINKLES), m_robotLED) + ); + public RobotContainer() { configureButtonBindings(); @@ -138,6 +150,8 @@ public class RobotContainer { // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) // .onTrue(new RotTo45(m_robotSwerveDrive)); + new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) + .onTrue(RobotShoot); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); @@ -152,8 +166,11 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.deactivateLuigiMode())); - } + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); + + } + +//.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT))); /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index a6c72ad..7c55826 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,13 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 22; - public static final String GIT_SHA = "e2797ab77871b91d546e6e9793852cf94d8b712f"; - public static final String GIT_DATE = "2026-01-27 19:28:57 MST"; + public static final int GIT_REVISION = 23; + public static final String GIT_SHA = "8bda61b9837d7450a2fb4650bf02ffb1f4f06213"; + public static final String GIT_DATE = "2026-01-29 16:59:53 MST"; public static final String GIT_BRANCH = "Subsystem-Boilerplate"; - public static final String BUILD_DATE = "2026-01-29 16:59:21 MST"; - public static final long BUILD_UNIX_TIME = 1769731161482L; - public static final int DIRTY = 1; + public static final String BUILD_DATE = "2026-01-29 17:04:31 MST"; + public static final long BUILD_UNIX_TIME = 1769731471423L; + public static final int DIRTY = 0; private BuildConstants(){} } diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index c050278..e095b06 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -30,7 +30,7 @@ public class LED extends SubsystemBase implements Queryable { private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); private LEDPatterns mode = LEDConstants.DEFAULT_PATTERN; - +//hello public void setMode(LEDPatterns pattern){ this.mode = pattern; } diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 1aac079..0693e0b 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -34,6 +34,7 @@ public class Intake extends SubsystemBase { switch (mode) { case Up: io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER); + io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP); break; case Down: io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 48600d5..0a1236d 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -33,7 +33,9 @@ public class IntakeConstants { // negative is left, positive is right public static final Angle ARM_LIMIT_LOWER = Degrees.of(-180); public static final Angle ARM_LIMIT_UPPER = Degrees.of(180); - public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(0.0); + public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0); + public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0); + public static final Slot0Configs ARM_PID = new Slot0Configs() .withKP(2.0) diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 60ddaec..0c879f4 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -1,7 +1,5 @@ package frc4388.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.Rotation; - import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -9,7 +7,8 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.utility.status.FaultReporter; +import frc4388.robot.subsystems.intake.IntakeConstants; +import frc4388.robot.subsystems.shooter.ShooterIO.ShooterState; public class Shooter extends SubsystemBase { ShooterIO io; @@ -17,6 +16,7 @@ public class Shooter extends SubsystemBase { Supplier m_swervePoseSupplier; + public Shooter( ShooterIO io, Supplier swervePoseSupplier @@ -33,12 +33,43 @@ public class Shooter extends SubsystemBase { } + + public enum ShooterMode { + //Shooter is at speed it fires balls + Active, + //Shooter is at a resting velocity + Resting, + //Shooter is inactive (Off) + Inactive, + } + + public void setMode(ShooterMode mode) { + switch (mode) { + case Active: + io.setMotor1Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY); + io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY); + io.setIndexerVelocity(state, ShooterConstants.INDEXER_ACTIVE_VELOCITY); + break; + case Resting: + io.setMotor1Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); + io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); + io.setIndexerVelocity(state, ShooterConstants.INDEXER_INACTIVE_VELOCITY); + break; + case Inactive: + io.setMotor1Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); + io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); + io.setIndexerVelocity(state, ShooterConstants.INDEXER_INACTIVE_VELOCITY); + break; + } + } + // Calculate what should be done based off of the position of the robot // TODO: Implement field zones public FieldZone getTarget(Pose2d position) { return FieldZone.InShootZone; } + @Override public void periodic() { diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 1e80500..7e167cd 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -1,6 +1,7 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; @@ -9,6 +10,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import frc4388.utility.status.CanDevice; public class ShooterConstants { @@ -19,6 +21,15 @@ public class ShooterConstants { public static final double SHOOTERMOTOR2_GEAR_RATIO = 1.; public static final double INDEXER_GEAR_RATIO = 1.; + public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(0.0); + public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(0.0); + public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); + public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(0.0); + public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); + + + + public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) .withKP(0.0) From 6c082c7103ab547b92d7d50c9849eec955dd4b6b Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Thu, 29 Jan 2026 18:27:18 -0800 Subject: [PATCH 14/29] Climber subsystem unconfirmed subsytem - incomplete,no motors mapped --- .../robot/constants/BuildConstants.java | 12 +-- .../robot/subsystems/climber/Climber.java | 79 +++++++++++++++++++ .../subsystems/climber/ClimberConstants.java | 74 +++++++++++++++++ .../robot/subsystems/climber/ClimberIO.java | 43 ++++++++++ .../robot/subsystems/climber/ClimberReal.java | 78 ++++++++++++++++++ 5 files changed, 280 insertions(+), 6 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/climber/Climber.java create mode 100644 src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java create mode 100644 src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java create mode 100644 src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 7c55826..40d825f 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,13 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 23; - public static final String GIT_SHA = "8bda61b9837d7450a2fb4650bf02ffb1f4f06213"; - public static final String GIT_DATE = "2026-01-29 16:59:53 MST"; + public static final int GIT_REVISION = 24; + public static final String GIT_SHA = "5d381731689602f5fac161f1552170003ef30c14"; + public static final String GIT_DATE = "2026-01-29 18:07:19 MST"; public static final String GIT_BRANCH = "Subsystem-Boilerplate"; - public static final String BUILD_DATE = "2026-01-29 17:04:31 MST"; - public static final long BUILD_UNIX_TIME = 1769731471423L; - public static final int DIRTY = 0; + public static final String BUILD_DATE = "2026-01-29 19:26:44 MST"; + public static final long BUILD_UNIX_TIME = 1769740004409L; + public static final int DIRTY = 1; private BuildConstants(){} } diff --git a/src/main/java/frc4388/robot/subsystems/climber/Climber.java b/src/main/java/frc4388/robot/subsystems/climber/Climber.java new file mode 100644 index 0000000..f667f1b --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/climber/Climber.java @@ -0,0 +1,79 @@ +package frc4388.robot.subsystems.climber; + +import static edu.wpi.first.units.Units.Rotation; + +import java.util.function.Supplier; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.utility.status.FaultReporter; + +public class Climber extends SubsystemBase { + ClimberIO io; + ClimberStateAutoLogged state = new ClimberStateAutoLogged(); + + Supplier m_swervePoseSupplier; + + public Climber( + ClimberIO io, + Supplier swervePoseSupplier + ) { + this.io = io; + this.m_swervePoseSupplier = swervePoseSupplier; + } + + // public enum ClimberMode { + // Up, + // Down, + // } + + // public void setMode(IntakeMode mode) { + // switch (mode) { + // case Up: + // io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER); + // io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP); + // break; + // case Down: + // io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER); + // io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY); + // break; + // } + // } + + + // public enum FieldZone { + // // The robot should aim at the hub + // InShootZone, + // // The robot should aim towards the wall + // AimAtWall, + + // } + + // // Calculate what should be done based off of the position of the robot + // // TODO: Implement field zones + // public FieldZone getTarget(Pose2d position) { + // return FieldZone.InShootZone; + // } + + @Override + public void periodic() { + + + + // FaultReporter.register(this); // TODO Implement fault reporter + + + Logger.processInputs("Climber", state); + + Pose2d pose = m_swervePoseSupplier.get(); + Angle robotRot = pose.getRotation().getMeasure(); + + io.updateInputs(state); + + } +} + + diff --git a/src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java new file mode 100644 index 0000000..c0b5329 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java @@ -0,0 +1,74 @@ +package frc4388.robot.subsystems.climber; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import frc4388.utility.status.CanDevice; + +public class ClimberConstants { + // Motor conversions + + public static final double CLIMBER_MOTOR_GEAR_RATIO = 1.; + + //IDs + + + // public static final CanDevice CLIMBER_ID = new CanDevice("SHOOTER 1", 20); + + // Limits + + // 0 is the forward angle on the robot. + // negative is left, positive is right + // public static final Angle L1 = Degrees.of(-180); + // public static final Angle ARM_LIMIT_UPPER = Degrees.of(180); + public static final Distance CLIMBER_LIMIT_LOWER = Inches.of(0); + + public static final Distance CLIMBER_LIMIT_UPPER = Inches.of(0); + + + public static final Slot0Configs CLIMBER_PID = new Slot0Configs() + .withKP(2.0) + .withKI(0.0) + .withKD(10.0); + + // 0 is paralell to the ground, 90 is directly up + // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); + // public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); + + // Motor configs + public static final TalonFXConfiguration CLIMBER_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); +} + + // public static final class IDs { + // public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22); + // } + // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() + // .withCurrentLimits( + // new CurrentLimitsConfigs() + // .withStatorCurrentLimit(40) // TODO: tune??? + // .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means + // ).withMotorOutput( + // new MotorOutputConfigs() + // .withNeutralMode(NeutralModeValue.Brake) // Brake so it stop + // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + // ); diff --git a/src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java new file mode 100644 index 0000000..bddb5fc --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java @@ -0,0 +1,43 @@ +package frc4388.robot.subsystems.climber; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Inches; + +import org.littletonrobotics.junction.AutoLog; + +import edu.wpi.first.units.CurrentUnit; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; + +public interface ClimberIO { + @AutoLog + public class ClimberState { + Distance climberDistance = Inches.of(0); + Distance climberTargetDistance = Inches.of(0); + Current climberMotorCurrent = Amps.of(0); + + // Distance shooterPitch = Rotations.of(0); + // Angle shooterTargetPitch = Rotations.of(0); + // Current pitchMotorCurrent = Amps.of(0); + + // AngularVelocity rollerVelocity = RotationsPerSecond.of(0); + // AngularVelocity rollerTargetVelocity = RotationsPerSecond.of(0); + // Current rollerMotorCurrent = Amps.of(0); + + // LinearVelocity feederVelocity = InchesPerSecond.of(0); + // LinearVelocity feederTargetVelocity = InchesPerSecond.of(0); + // Current feederMotorCurrent = Amps.of(0); + } + + // public default void setShooterAngle(ShooterState state, Angle angle) {} + // public default void setShooterPitch(ShooterState state, Angle angle) {} + public default void setClimberDistance(ClimberState state, Distance distance) {} + + public default void updateInputs(ClimberState state) {} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java b/src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java new file mode 100644 index 0000000..2345a08 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java @@ -0,0 +1,78 @@ +package frc4388.robot.subsystems.climber; + +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; + +import edu.wpi.first.units.measure.*; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.subsystems.intake.IntakeConstants; +import frc4388.utility.configurable.ConfigurableDouble; + +public class ClimberReal implements ClimberIO { + + + TalonFX m_climberMotor; + + public ClimberReal( + + TalonFX climberMotor + ) { + // m_angleMotor = angleMotor; + // m_pitchMotor = pitchMotor; + m_climberMotor = climberMotor; + // Apply the configs + m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_PID); + m_climberMotor.getConfigurator().apply(ClimberConstants.CLIMBER_MOTOR_CONFIG); + + } + + private Distance clampDistance(Distance distance, Distance climberLimitLower, Distance climberLimitUpper){ + if(distance.gt(climberLimitUpper)) { + return climberLimitUpper; + }else if(distance.lt(climberLimitLower)) { + return climberLimitLower; + }else{ + return distance; + } + } + + + + + @Override + public void setClimberDistance(ClimberState state, Distance distance) { + state.climberTargetDistance = distance; + // Assume that the angle is always accurate, because I think we will use a shaft encoder + // Assume that 0 degrees = forwards. Might need an offset here + + Distance boundedDistance = clampDistance(distance, ClimberConstants.CLIMBER_LIMIT_LOWER, ClimberConstants.CLIMBER_LIMIT_UPPER); + // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT + double motorTargetDistance = boundedDistance.in(Inches) / ClimberConstants.CLIMBER_MOTOR_GEAR_RATIO; + PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetDistance); + m_climberMotor.setControl(posRequest); + } + + @Override + public void updateInputs(ClimberState state) { + double motorRotations = m_climberMotor.getPosition().getValue().in(Rotations); + double linearInches = motorRotations * ClimberConstants.CLIMBER_MOTOR_GEAR_RATIO; + state.climberDistance = Inches.of(linearInches); + state.climberMotorCurrent = m_climberMotor.getStatorCurrent(false).getValue(); + + // state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO); + // state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue(); + + // state.armAngle = m_armMotor.getPosition().getValue(); + // state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue(); + + + } +} From 48aa289b4e3fa64f0bd9dffdc9a7913e3c811b72 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Thu, 29 Jan 2026 19:30:50 -0700 Subject: [PATCH 15/29] Mira goofy test --- .../java/frc4388/robot/RobotContainer.java | 10 +++++++++- .../robot/constants/BuildConstants.java | 12 +++++------ .../frc4388/robot/constants/Constants.java | 4 +--- .../java/frc4388/robot/subsystems/LED.java | 6 +++++- .../robot/subsystems/vision/Vision.java | 20 +++++++++++++------ .../configurable/ConfigurableString.java | 2 ++ 6 files changed, 37 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9d1cf19..a4df644 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -42,6 +42,7 @@ import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; import frc4388.utility.compute.TimesNegativeOne; +import frc4388.utility.configurable.ConfigurableString; import frc4388.utility.controller.DeadbandedXboxController; // Autos import frc4388.utility.controller.VirtualController; @@ -62,6 +63,8 @@ public class RobotContainer { /* Subsystems */ public final LED m_robotLED = new LED(); + //Testing of Colors + public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); private Boolean operatorManualMode = false; @@ -90,7 +93,12 @@ public class RobotContainer { private Command autoCommand; private Command RobotShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.PARTY_TWINKLES), m_robotLED) + new InstantCommand(() -> System.out.println(m_robotLED.getMode())), + new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.PARTY_TWINKLES), m_robotLED), + new InstantCommand(() -> System.out.println(m_robotLED.getMode())), + new WaitCommand(5), + new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_RED), m_robotLED), + new InstantCommand(() -> System.out.println(m_robotLED.getMode())) ); public RobotContainer() { diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 7c55826..650a3a1 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,13 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 23; - public static final String GIT_SHA = "8bda61b9837d7450a2fb4650bf02ffb1f4f06213"; - public static final String GIT_DATE = "2026-01-29 16:59:53 MST"; + public static final int GIT_REVISION = 24; + public static final String GIT_SHA = "5d381731689602f5fac161f1552170003ef30c14"; + public static final String GIT_DATE = "2026-01-29 18:07:19 MST"; public static final String GIT_BRANCH = "Subsystem-Boilerplate"; - public static final String BUILD_DATE = "2026-01-29 17:04:31 MST"; - public static final long BUILD_UNIX_TIME = 1769731471423L; - public static final int DIRTY = 0; + public static final String BUILD_DATE = "2026-01-29 19:27:45 MST"; + public static final long BUILD_UNIX_TIME = 1769740065495L; + public static final int DIRTY = 1; private BuildConstants(){} } diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index dd7b8af..490aefd 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -7,8 +7,6 @@ package frc4388.robot.constants; -import com.ctre.phoenix6.configs.Slot0Configs; - import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; @@ -21,7 +19,6 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import frc4388.utility.compute.Trim; -import frc4388.utility.status.CanDevice; import frc4388.utility.structs.Gains; import frc4388.utility.structs.LEDPatterns; @@ -91,6 +88,7 @@ public final class Constants { public static final Matrix kSingleTagStdDevs = VecBuilder.fill(0.5, 0.5, 4); public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.1, 0.1, 1); } + public static final class LEDConstants { public static final int LED_SPARK_ID = 9; diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index e095b06..9edfb9d 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -30,11 +30,15 @@ public class LED extends SubsystemBase implements Queryable { private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); private LEDPatterns mode = LEDConstants.DEFAULT_PATTERN; -//hello + public void setMode(LEDPatterns pattern){ this.mode = pattern; } + public String getMode(){ + return mode.name(); + } + @Override public void periodic() { update(); diff --git a/src/main/java/frc4388/robot/subsystems/vision/Vision.java b/src/main/java/frc4388/robot/subsystems/vision/Vision.java index fedc22a..9ab223e 100644 --- a/src/main/java/frc4388/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/vision/Vision.java @@ -2,15 +2,11 @@ package frc4388.robot.subsystems.vision; import java.util.ArrayList; import java.util.List; -import java.util.Optional; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.swerve.SwerveDrivetrain; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -18,20 +14,22 @@ import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; import frc4388.utility.status.FaultReporter; import frc4388.utility.status.Queryable; import frc4388.utility.status.Status; +import frc4388.utility.structs.LEDPatterns; -public class Vision extends SubsystemBase implements Queryable { +public class Vision extends SubsystemBase implements Queryable{ VisionIO[] io; VisionStateAutoLogged[] state; public Pose2d lastVisionPose = new Pose2d(); public Pose2d lastPhysOdomPose = new Pose2d(); + public LED m_robotLED; public Vision(VisionIO... devices) { FaultReporter.register(this); io = devices; state = new VisionStateAutoLogged[io.length]; - + m_robotLED = new LED(); for(int i = 0; i < io.length; i++) { state[i] = new VisionStateAutoLogged(); } @@ -44,6 +42,10 @@ public class Vision extends SubsystemBase implements Queryable { Logger.processInputs("Vision/Camera" + i , state[i]); } Logger.recordOutput("Vision/isTagDectected", isTag()); + + if (isTag()){ + m_robotLED.setMode(LEDPatterns.SOLID_GREEN_DARK); + } } public List getPosesToAdd(){ @@ -93,4 +95,10 @@ public class Vision extends SubsystemBase implements Queryable { // throw new UnsupportedOperationException("Unimplemented method 'diagnosticStatus'"); } + // Simple LED helper class for compilation and basic usage; replace with real implementation if available. + private static class LED { + public void setMode(LEDPatterns mode) { + // no-op stub for compilation; integrate with hardware driver as needed + } + } } diff --git a/src/main/java/frc4388/utility/configurable/ConfigurableString.java b/src/main/java/frc4388/utility/configurable/ConfigurableString.java index 34c0290..69b144b 100644 --- a/src/main/java/frc4388/utility/configurable/ConfigurableString.java +++ b/src/main/java/frc4388/utility/configurable/ConfigurableString.java @@ -11,6 +11,7 @@ public class ConfigurableString { * @param name the name of the Smart Dashboard key. * @param defualtValue the initilization value */ + public ConfigurableString(String name, String defualtValue) { this.name = name; this.defualtValue = defualtValue; @@ -20,4 +21,5 @@ public class ConfigurableString { public String get() { return SmartDashboard.getString(name, defualtValue); } + } From 5f4d7887ffc93ef535cef23511da086cd7f6d8bd Mon Sep 17 00:00:00 2001 From: Shikhar Date: Sat, 31 Jan 2026 15:33:14 -0700 Subject: [PATCH 16/29] Start on intake orientaiton --- .../robot/subsystems/swerve/SwerveDrive.java | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index bd1a358..80bcf5e 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -264,6 +264,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void driveFieldAngle(Translation2d leftStick, Rotation2d heading) { if (leftStick.getNorm() < 0.05) // if no imput and the swerve drive is still going: stopModules(); // stop the swerve + leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); @@ -285,6 +286,18 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // SmartDashboard.putBoolean("drift correction", true); } + + public void driveIntake(Translation2d leftStick, Rotation2d heading, boolean invert){ + if (invert){ + Translation2d stick = new Translation2d(-leftStick.getX(), -leftStick.getY()); + driveFieldAngle(stick, heading); + + } else{ + driveFieldAngle(leftStick, heading); + } + } + + // Drive with the robot facing towards a specific position public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) { From dc33af165d04720f576a5d490987d6ee079c4b8d Mon Sep 17 00:00:00 2001 From: Shikhar Date: Sat, 31 Jan 2026 21:38:10 -0700 Subject: [PATCH 17/29] driveIntake --- .../robot/constants/BuildConstants.java | 10 ++++----- .../robot/subsystems/swerve/SwerveDrive.java | 21 ++++++++++++------- 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 650a3a1..2150560 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 24; - public static final String GIT_SHA = "5d381731689602f5fac161f1552170003ef30c14"; - public static final String GIT_DATE = "2026-01-29 18:07:19 MST"; + public static final int GIT_REVISION = 28; + public static final String GIT_SHA = "5f4d7887ffc93ef535cef23511da086cd7f6d8bd"; + public static final String GIT_DATE = "2026-01-31 15:33:14 MST"; public static final String GIT_BRANCH = "Subsystem-Boilerplate"; - public static final String BUILD_DATE = "2026-01-29 19:27:45 MST"; - public static final long BUILD_UNIX_TIME = 1769740065495L; + public static final String BUILD_DATE = "2026-01-31 21:37:04 MST"; + public static final long BUILD_UNIX_TIME = 1769920624950L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 80bcf5e..f3de1be 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -287,14 +287,21 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } - public void driveIntake(Translation2d leftStick, Rotation2d heading, boolean invert){ - if (invert){ - Translation2d stick = new Translation2d(-leftStick.getX(), -leftStick.getY()); - driveFieldAngle(stick, heading); + public void driveIntake(Translation2d leftStick){ + // if (invert){ + // Translation2d stick = new Translation2d(-leftStick.getX(), -leftStick.getY()); + // driveFieldAngle(stick, heading); - } else{ - driveFieldAngle(leftStick, heading); - } + // } else{ + // driveFieldAngle(leftStick, heading); + // } + double speed = leftStick.getNorm(); + + Rotation2d heading = leftStick.getAngle(); + + // Only drive forward in robot direction (no strafe) + Translation2d forwardOnly = new Translation2d(speed, 0.0); + driveFieldAngle(forwardOnly, heading); } From de0952c14a77e2b7da51b7af3a3833d495c2a3f4 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Mon, 2 Feb 2026 19:41:24 -0700 Subject: [PATCH 18/29] Configuration --- .../java/frc4388/robot/RobotContainer.java | 61 ++++++++++++------- src/main/java/frc4388/robot/RobotMap.java | 40 ++++++------ .../robot/constants/BuildConstants.java | 10 +-- .../robot/subsystems/swerve/SwerveDrive.java | 1 + .../swerve/SwerveDriveConstants.java | 11 ++-- 5 files changed, 72 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a4df644..9c80061 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -11,6 +11,7 @@ import java.io.File; import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; @@ -19,30 +20,23 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // Commands import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc4388.robot.commands.MoveForTimeCommand; -import frc4388.robot.commands.MoveUntilSuply; import frc4388.robot.constants.Constants; -import frc4388.robot.constants.Constants.AutoConstants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; import frc4388.robot.constants.FieldConstants; // Subsystems import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.shooter.Shooter; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; import frc4388.utility.compute.TimesNegativeOne; -import frc4388.utility.configurable.ConfigurableString; import frc4388.utility.controller.DeadbandedXboxController; // Autos import frc4388.utility.controller.VirtualController; @@ -65,7 +59,7 @@ public class RobotContainer { public final LED m_robotLED = new LED(); //Testing of Colors - public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera); + public final Vision m_vision = new Vision(); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); private Boolean operatorManualMode = false; // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -116,19 +110,10 @@ public class RobotContainer { DriverStation.silenceJoystickConnectionWarning(true); m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - - // IF the driver is holding the aim button, aim the robot towards the hub - if(m_driverXbox.getRightTriggerAxis() > 0.5) { - // Aim - m_robotSwerveDrive.driveFacingPosition( - getDeadbandedDriverController().getLeft(), - FieldConstants.BLUE_HUB_POS); - } else { - // Drive normally - m_robotSwerveDrive.driveWithInput( - getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight(),true); - } + // Drive normally + m_robotSwerveDrive.driveWithInput( + getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight(),true); }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); @@ -170,12 +155,46 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); + // IF the driver is holding the aim button, aim the robot towards the hub + new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) + .whileTrue(new RunCommand( + () -> { + m_robotSwerveDrive.driveFacingPosition( + getDeadbandedDriverController().getLeft(), + FieldConstants.BLUE_HUB_POS); + }, m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); + + + // IF the driver is holding the aim button, aim the robot towards the hub + new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) + .whileTrue(new RunCommand( + () -> { + m_robotSwerveDrive.driveIntake( + getDeadbandedDriverController().getLeft() + ); + }, m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); + + // D-PAD fine alignment + new Trigger(() -> getDeadbandedDriverController().getPOV() != -1) + .whileTrue(new RunCommand( + () -> m_robotSwerveDrive.driveFine( + new Translation2d( + 1, + Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV()) + ), + getDeadbandedDriverController().getRight(), 0.15 + ), m_robotSwerveDrive)) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); } //.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT))); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index f0a4400..10bc3bc 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -46,8 +46,8 @@ public class RobotMap { // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id); // public RobotGyro gyro = new RobotGyro(m_pigeon2); - public final VisionIO leftCamera; - public final VisionIO rightCamera; + // public final VisionIO leftCamera; + // public final VisionIO rightCamera; // public final LiDAR lidar = new @@ -62,21 +62,21 @@ public class RobotMap { public final SwerveIO swerveDrivetrain; // /* Elevator Subsystem */ - public final ShooterIO shooterIO; - public final IntakeIO intakeIO; + // public final ShooterIO shooterIO; + // public final IntakeIO intakeIO; public RobotMap(SimConstants.Mode mode) { switch (mode) { case REAL: // Configure cameras - PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); - PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); + // PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); + // PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); - leftCamera = new VisionReal(leftCameraReal, VisionConstants.RIGHT_CAMERA_POS); ; - rightCamera = new VisionReal(rightCameraReal, VisionConstants.LEFT_CAMERA_POS); + // leftCamera = new VisionReal(leftCameraReal, VisionConstants.RIGHT_CAMERA_POS); ; + // rightCamera = new VisionReal(rightCameraReal, VisionConstants.LEFT_CAMERA_POS); - FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera"); - FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera"); + // FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera"); + // FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera"); // // Configure LiDAR // reefLidar = new LidarReal(LiDARConstants.REEF_LIDAR_DIO_CHANNEL); @@ -98,16 +98,16 @@ public class RobotMap { TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id); //Configure Intake - TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id); - TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id); + // TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id); + // TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id); // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - shooterIO = new ShooterReal(shooter1, shooter2, indexer); + // shooterIO = new ShooterReal(shooter1, shooter2, indexer); - intakeIO = new IntakeReal(arm, roller); + // intakeIO = new IntakeReal(arm, roller); // Fault FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); @@ -116,8 +116,8 @@ public class RobotMap { FaultTalonFX.addDevice(shooter1, "Shooter1"); FaultTalonFX.addDevice(shooter2, "Shooter2"); FaultTalonFX.addDevice(indexer, "Indexer"); - FaultTalonFX.addDevice(arm, "Arm"); - FaultTalonFX.addDevice(roller, "Roller"); + // FaultTalonFX.addDevice(arm, "Arm"); + // FaultTalonFX.addDevice(roller, "Roller"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); @@ -136,11 +136,11 @@ public class RobotMap { // case SIM: // break; default: - leftCamera = new VisionIO() {}; - rightCamera = new VisionIO() {}; + // leftCamera = new VisionIO() {}; + // rightCamera = new VisionIO() {}; swerveDrivetrain = new SwerveIO() {}; - intakeIO = new IntakeIO() {}; - shooterIO = new ShooterIO() {}; + // intakeIO = new IntakeIO() {}; + // shooterIO = new ShooterIO() {}; break; } } diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 2150560..fc4bd88 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 28; - public static final String GIT_SHA = "5f4d7887ffc93ef535cef23511da086cd7f6d8bd"; - public static final String GIT_DATE = "2026-01-31 15:33:14 MST"; + public static final int GIT_REVISION = 29; + public static final String GIT_SHA = "dc33af165d04720f576a5d490987d6ee079c4b8d"; + public static final String GIT_DATE = "2026-01-31 21:38:10 MST"; public static final String GIT_BRANCH = "Subsystem-Boilerplate"; - public static final String BUILD_DATE = "2026-01-31 21:37:04 MST"; - public static final long BUILD_UNIX_TIME = 1769920624950L; + public static final String BUILD_DATE = "2026-02-02 19:39:42 MST"; + public static final long BUILD_UNIX_TIME = 1770086382841L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index f3de1be..046ea38 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -287,6 +287,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } + public void driveIntake(Translation2d leftStick){ // if (invert){ // Translation2d stick = new Translation2d(-leftStick.getX(), -leftStick.getY()); diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index 9318444..cee7d43 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -31,6 +31,7 @@ import frc4388.utility.structs.Gains; // No mans land // Beware, there be dragons. public final class SwerveDriveConstants { + public static final String CANBUS_NAME = "drivetrain"; public static final double MAX_ROT_SPEED = Math.PI * 2; public static final double AUTO_MAX_ROT_SPEED = 1.5; public static final double MIN_ROT_SPEED = 1.0; @@ -79,7 +80,7 @@ public final class SwerveDriveConstants { private static final class ModuleSpecificConstants { //2025 //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(-0.368896484375); + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.35); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; @@ -87,7 +88,7 @@ public final class SwerveDriveConstants { private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(-0.011474609375); + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3); private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; @@ -95,7 +96,7 @@ public final class SwerveDriveConstants { private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.333251953125+0.5); + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; @@ -103,7 +104,7 @@ public final class SwerveDriveConstants { private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.4306640625+0.5); + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; @@ -208,7 +209,7 @@ public final class SwerveDriveConstants { } public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withPigeon2Id(IDs.DRIVE_PIGEON.id); + .withPigeon2Id(IDs.DRIVE_PIGEON.id).withCANBusName(CANBUS_NAME); private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() // holy verbosity batman. From 9fcc5ffe9f52bb7c311e0cb573061158ec2794ce Mon Sep 17 00:00:00 2001 From: Shikhar Date: Mon, 2 Feb 2026 19:46:18 -0700 Subject: [PATCH 19/29] Calibration --- .../java/frc4388/robot/constants/BuildConstants.java | 10 +++++----- .../robot/subsystems/swerve/SwerveDriveConstants.java | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index fc4bd88..6548068 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 29; - public static final String GIT_SHA = "dc33af165d04720f576a5d490987d6ee079c4b8d"; - public static final String GIT_DATE = "2026-01-31 21:38:10 MST"; + public static final int GIT_REVISION = 30; + public static final String GIT_SHA = "de0952c14a77e2b7da51b7af3a3833d495c2a3f4"; + public static final String GIT_DATE = "2026-02-02 19:41:24 MST"; public static final String GIT_BRANCH = "Subsystem-Boilerplate"; - public static final String BUILD_DATE = "2026-02-02 19:39:42 MST"; - public static final long BUILD_UNIX_TIME = 1770086382841L; + public static final String BUILD_DATE = "2026-02-02 19:42:58 MST"; + public static final long BUILD_UNIX_TIME = 1770086578285L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index cee7d43..5993003 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -96,7 +96,7 @@ public final class SwerveDriveConstants { private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438); + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; From 7b46b7a1a30820e2e754c8b8335bd4207dcddf77 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Tue, 3 Feb 2026 16:36:09 -0800 Subject: [PATCH 20/29] Better swereve offsets --- .../java/frc4388/robot/constants/BuildConstants.java | 10 +++++----- .../robot/subsystems/swerve/SwerveDriveConstants.java | 8 ++++---- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 6548068..1903dd6 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 30; - public static final String GIT_SHA = "de0952c14a77e2b7da51b7af3a3833d495c2a3f4"; - public static final String GIT_DATE = "2026-02-02 19:41:24 MST"; + public static final int GIT_REVISION = 31; + public static final String GIT_SHA = "9fcc5ffe9f52bb7c311e0cb573061158ec2794ce"; + public static final String GIT_DATE = "2026-02-02 19:46:18 MST"; public static final String GIT_BRANCH = "Subsystem-Boilerplate"; - public static final String BUILD_DATE = "2026-02-02 19:42:58 MST"; - public static final long BUILD_UNIX_TIME = 1770086578285L; + public static final String BUILD_DATE = "2026-02-03 17:30:18 MST"; + public static final long BUILD_UNIX_TIME = 1770165018277L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index 5993003..af2f6fc 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -80,7 +80,7 @@ public final class SwerveDriveConstants { private static final class ModuleSpecificConstants { //2025 //Front Left - private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.35); + private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean FRONT_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_LEFT_ENCODER_INVERTED = false; @@ -88,7 +88,7 @@ public final class SwerveDriveConstants { private static final Distance FRONT_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Front Right - private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3); + private static final Angle FRONT_RIGHT_ENCODER_OFFSET = Rotations.of(0.465332+0.3+0.003174-0.0103); private static final boolean FRONT_RIGHT_DRIVE_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean FRONT_RIGHT_ENCODER_INVERTED = false; @@ -96,7 +96,7 @@ public final class SwerveDriveConstants { private static final Distance FRONT_RIGHT_YPOS = Inches.of(-HALF_HEIGHT); //Back Left - private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5); + private static final Angle BACK_LEFT_ENCODER_OFFSET = Rotations.of(0.023438+0.5+0.0168-0.00562); private static final boolean BACK_LEFT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_LEFT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_LEFT_ENCODER_INVERTED = false; @@ -104,7 +104,7 @@ public final class SwerveDriveConstants { private static final Distance BACK_LEFT_YPOS = Inches.of(HALF_HEIGHT); //Back Right - private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05); + private static final Angle BACK_RIGHT_ENCODER_OFFSET = Rotations.of(0.029541+0.05-0.002197-0.00366); private static final boolean BACK_RIGHT_DRIVE_MOTOR_INVERTED = false; private static final boolean BACK_RIGHT_STEER_MOTOR_INVERTED = true; private static final boolean BACK_RIGHT_ENCODER_INVERTED = false; From f39bd20b9f69f52fc145a3686091744706b3272e Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Tue, 3 Feb 2026 17:06:32 -0800 Subject: [PATCH 21/29] Intake Finished --- .../java/frc4388/robot/RobotContainer.java | 3 ++- .../robot/constants/BuildConstants.java | 10 ++++----- .../robot/subsystems/swerve/SwerveDrive.java | 21 ++++++++++++++----- 3 files changed, 23 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9c80061..e317919 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -179,7 +179,8 @@ public class RobotContainer { .whileTrue(new RunCommand( () -> { m_robotSwerveDrive.driveIntake( - getDeadbandedDriverController().getLeft() + getDeadbandedDriverController().getLeft(), + false ); }, m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 1903dd6..d34231e 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 31; - public static final String GIT_SHA = "9fcc5ffe9f52bb7c311e0cb573061158ec2794ce"; - public static final String GIT_DATE = "2026-02-02 19:46:18 MST"; + public static final int GIT_REVISION = 32; + public static final String GIT_SHA = "7b46b7a1a30820e2e754c8b8335bd4207dcddf77"; + public static final String GIT_DATE = "2026-02-03 17:36:09 MST"; public static final String GIT_BRANCH = "Subsystem-Boilerplate"; - public static final String BUILD_DATE = "2026-02-03 17:30:18 MST"; - public static final long BUILD_UNIX_TIME = 1770165018277L; + public static final String BUILD_DATE = "2026-02-03 18:04:39 MST"; + public static final long BUILD_UNIX_TIME = 1770167079826L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 046ea38..511fb4e 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -288,7 +288,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { - public void driveIntake(Translation2d leftStick){ + public void driveIntake(Translation2d leftStick, boolean invertRotation){ // if (invert){ // Translation2d stick = new Translation2d(-leftStick.getX(), -leftStick.getY()); // driveFieldAngle(stick, heading); @@ -298,11 +298,22 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // } double speed = leftStick.getNorm(); - Rotation2d heading = leftStick.getAngle(); + if(speed < 0.3) { + driveWithInput(leftStick, new Translation2d(), true); + } else { - // Only drive forward in robot direction (no strafe) - Translation2d forwardOnly = new Translation2d(speed, 0.0); - driveFieldAngle(forwardOnly, heading); + + + Rotation2d heading = new Rotation2d(leftStick.getX(), -leftStick.getY());//.r otateBy(Rotation2d.fromDegrees(90)); + + // if (invertRotation){ + heading = heading.rotateBy(Rotation2d.fromDegrees(270)); + // } + + // Only drive forward in robot direction (no strafe) + // Translation2d forwardOnly = new Translation2d(speed, 0.0); + driveFieldAngle(leftStick, heading); + } } From 2d9ed527bebbf79d7643452fe4344ad32b3fab49 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Sat, 7 Feb 2026 14:51:05 -0700 Subject: [PATCH 22/29] instant commands for testing --- .../java/frc4388/robot/RobotContainer.java | 40 +++++++++++++------ src/main/java/frc4388/robot/RobotMap.java | 26 ++++++------ .../robot/subsystems/intake/Intake.java | 10 ++--- .../subsystems/intake/IntakeConstants.java | 8 ++-- .../robot/subsystems/shooter/Shooter.java | 12 +++--- .../subsystems/shooter/ShooterConstants.java | 6 ++- 6 files changed, 61 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e317919..70c894c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -33,6 +33,8 @@ import frc4388.robot.constants.Constants.SimConstants.Mode; import frc4388.robot.constants.FieldConstants; // Subsystems import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.intake.Intake; +import frc4388.robot.subsystems.shooter.Shooter; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; @@ -58,9 +60,10 @@ public class RobotContainer { /* Subsystems */ public final LED m_robotLED = new LED(); //Testing of Colors - public final Vision m_vision = new Vision(); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); + public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO); + public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO); private Boolean operatorManualMode = false; // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -86,13 +89,21 @@ public class RobotContainer { private SendableChooser autoChooser; private Command autoCommand; + // private Command RobotShoot = new SequentialCommandGroup( + // new InstantCommand(() -> System.out.println(m_robotLED.getMode())), + // new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.PARTY_TWINKLES), m_robotLED), + // new InstantCommand(() -> System.out.println(m_robotLED.getMode())), + // new WaitCommand(5), + // new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_RED), m_robotLED), + // new InstantCommand(() -> System.out.println(m_robotLED.getMode())) + // ); + private Command RobotShoot = new SequentialCommandGroup( - new InstantCommand(() -> System.out.println(m_robotLED.getMode())), - new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.PARTY_TWINKLES), m_robotLED), - new InstantCommand(() -> System.out.println(m_robotLED.getMode())), - new WaitCommand(5), - new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_RED), m_robotLED), - new InstantCommand(() -> System.out.println(m_robotLED.getMode())) + new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Active), m_robotShooter) + ); + + private Command RobotIntake = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Down), m_robotIntake) ); public RobotContainer() { @@ -144,7 +155,12 @@ public class RobotContainer { // .onTrue(new RotTo45(m_robotSwerveDrive)); new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) - .onTrue(RobotShoot); + .onTrue(RobotShoot) + .onFalse(new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Resting), m_robotShooter)); + + new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode) + .onTrue(RobotIntake) + .onFalse(new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Up), m_robotShooter)); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); @@ -157,11 +173,11 @@ public class RobotContainer { - new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); - new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); // IF the driver is holding the aim button, aim the robot towards the hub new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 10bc3bc..cabf7b1 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -61,9 +61,9 @@ public class RobotMap { /* Swreve Drive Subsystem */ public final SwerveIO swerveDrivetrain; - // /* Elevator Subsystem */ - // public final ShooterIO shooterIO; - // public final IntakeIO intakeIO; + // /* Shooter and Intake Subsystem */ + public final ShooterIO shooterIO; + public final IntakeIO intakeIO; public RobotMap(SimConstants.Mode mode) { switch (mode) { @@ -91,23 +91,23 @@ public class RobotMap { swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); - // Configure Shooter + // Configure Shooter 22,23,24 TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id); TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id); TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id); - //Configure Intake - // TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id); - // TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id); + //Configure Intake 20,21 + TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id); + TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id); // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - // shooterIO = new ShooterReal(shooter1, shooter2, indexer); + shooterIO = new ShooterReal(shooter1, shooter2, indexer); - // intakeIO = new IntakeReal(arm, roller); + intakeIO = new IntakeReal(arm, roller); // Fault FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); @@ -116,8 +116,8 @@ public class RobotMap { FaultTalonFX.addDevice(shooter1, "Shooter1"); FaultTalonFX.addDevice(shooter2, "Shooter2"); FaultTalonFX.addDevice(indexer, "Indexer"); - // FaultTalonFX.addDevice(arm, "Arm"); - // FaultTalonFX.addDevice(roller, "Roller"); + FaultTalonFX.addDevice(arm, "Arm"); + FaultTalonFX.addDevice(roller, "Roller"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); @@ -139,8 +139,8 @@ public class RobotMap { // leftCamera = new VisionIO() {}; // rightCamera = new VisionIO() {}; swerveDrivetrain = new SwerveIO() {}; - // intakeIO = new IntakeIO() {}; - // shooterIO = new ShooterIO() {}; + intakeIO = new IntakeIO() {}; + shooterIO = new ShooterIO() {}; break; } } diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 0693e0b..c2ecd18 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -18,11 +18,11 @@ public class Intake extends SubsystemBase { Supplier m_swervePoseSupplier; public Intake( - IntakeIO io, - Supplier swervePoseSupplier + IntakeIO io + // Supplier swervePoseSupplier ) { this.io = io; - this.m_swervePoseSupplier = swervePoseSupplier; + // this.m_swervePoseSupplier = swervePoseSupplier; } public enum IntakeMode { @@ -33,11 +33,11 @@ public class Intake extends SubsystemBase { public void setMode(IntakeMode mode) { switch (mode) { case Up: - io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER); + // io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER); io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP); break; case Down: - io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER); + // io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER); io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY); break; } diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 0a1236d..197a789 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -23,7 +23,6 @@ public class IntakeConstants { //IDs - public static final CanDevice ARM_ID = new CanDevice("SHOOTER 1", 20); public static final CanDevice ROLLER_ID = new CanDevice("SHOOTER 2", 21); @@ -31,8 +30,11 @@ public class IntakeConstants { // 0 is the forward angle on the robot. // negative is left, positive is right - public static final Angle ARM_LIMIT_LOWER = Degrees.of(-180); - public static final Angle ARM_LIMIT_UPPER = Degrees.of(180); + + //when testing the negative output of 10% made the robot put the intake up + + public static final Angle ARM_LIMIT_LOWER = Degrees.of(90); + public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90); public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0); public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 0c879f4..2809bd2 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -14,15 +14,15 @@ public class Shooter extends SubsystemBase { ShooterIO io; ShooterStateAutoLogged state = new ShooterStateAutoLogged(); - Supplier m_swervePoseSupplier; + // Supplier m_swervePoseSupplier; public Shooter( - ShooterIO io, - Supplier swervePoseSupplier + ShooterIO io + // Supplier swervePoseSupplier ) { this.io = io; - this.m_swervePoseSupplier = swervePoseSupplier; + // this.m_swervePoseSupplier = swervePoseSupplier; } public enum FieldZone { @@ -80,8 +80,8 @@ public class Shooter extends SubsystemBase { Logger.processInputs("Shooter", state); - Pose2d pose = m_swervePoseSupplier.get(); - Angle robotRot = pose.getRotation().getMeasure(); + // Pose2d pose = m_swervePoseSupplier.get(); + // Angle robotRot = pose.getRotation().getMeasure(); io.updateInputs(state); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 7e167cd..64bab3e 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -22,9 +22,10 @@ public class ShooterConstants { public static final double INDEXER_GEAR_RATIO = 1.; public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(0.0); - public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(0.0); + + public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(4.0); public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); - public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(0.0); + public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(4.0); public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); @@ -59,6 +60,7 @@ public class ShooterConstants { // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means // ); + public static final CanDevice SHOOTER1_ID = new CanDevice("SHOOTER 1", 22); public static final CanDevice SHOOTER2_ID = new CanDevice("SHOOTER 2", 23); public static final CanDevice INDEXER_ID = new CanDevice("INDEXER",24); From 684e18ddcc5438f14f05c5902506d14cc4cbab19 Mon Sep 17 00:00:00 2001 From: Shikhar Date: Sat, 7 Feb 2026 14:52:21 -0700 Subject: [PATCH 23/29] quick fix --- .../java/frc4388/robot/constants/BuildConstants.java | 12 ++++++------ .../robot/subsystems/shooter/ShooterConstants.java | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index d34231e..e49a662 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,13 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 32; - public static final String GIT_SHA = "7b46b7a1a30820e2e754c8b8335bd4207dcddf77"; - public static final String GIT_DATE = "2026-02-03 17:36:09 MST"; + public static final int GIT_REVISION = 34; + public static final String GIT_SHA = "2d9ed527bebbf79d7643452fe4344ad32b3fab49"; + public static final String GIT_DATE = "2026-02-07 14:51:05 MST"; public static final String GIT_BRANCH = "Subsystem-Boilerplate"; - public static final String BUILD_DATE = "2026-02-03 18:04:39 MST"; - public static final long BUILD_UNIX_TIME = 1770167079826L; - public static final int DIRTY = 1; + public static final String BUILD_DATE = "2026-02-07 14:51:24 MST"; + public static final long BUILD_UNIX_TIME = 1770501084043L; + public static final int DIRTY = 0; private BuildConstants(){} } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 64bab3e..34c5b76 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -23,9 +23,9 @@ public class ShooterConstants { public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(0.0); - public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(4.0); + public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(-15); public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); - public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(4.0); + public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(0); public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); From d90bddac0fff1c8f1bb6dd9a6b4a862c822a005a Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Mon, 9 Feb 2026 16:03:48 -0800 Subject: [PATCH 24/29] Intake code ready for robot --- src/main/java/frc4388/robot/RobotMap.java | 26 ++++------ .../robot/constants/BuildConstants.java | 14 +++--- .../robot/subsystems/intake/Intake.java | 10 +--- .../subsystems/intake/IntakeConstants.java | 23 ++------- .../robot/subsystems/intake/IntakeReal.java | 49 +++++++++++++------ .../robot/subsystems/shooter/ShooterReal.java | 9 ++-- 6 files changed, 61 insertions(+), 70 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index cabf7b1..ea8f489 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -9,17 +9,10 @@ package frc4388.robot; import com.ctre.phoenix6.hardware.TalonFX; -import org.photonvision.PhotonCamera; -import org.photonvision.simulation.PhotonCameraSim; -import org.photonvision.simulation.SimCameraProperties; - import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.swerve.SwerveDrivetrain; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DigitalInput; //import frc4388.robot.constants.Constants.ElevatorConstants; import frc4388.robot.constants.Constants.SimConstants; -import frc4388.robot.constants.Constants.VisionConstants; import frc4388.robot.subsystems.intake.IntakeConstants; import frc4388.robot.subsystems.intake.IntakeIO; import frc4388.robot.subsystems.intake.IntakeReal; @@ -31,10 +24,7 @@ import frc4388.robot.subsystems.shooter.ShooterReal; import frc4388.robot.subsystems.swerve.SwerveDriveConstants; import frc4388.robot.subsystems.swerve.SwerveIO; import frc4388.robot.subsystems.swerve.SwerveReal; -import frc4388.robot.subsystems.vision.VisionIO; -import frc4388.robot.subsystems.vision.VisionReal; import frc4388.utility.status.FaultCANCoder; -import frc4388.utility.status.FaultPhotonCamera; import frc4388.utility.status.FaultPidgeon2; import frc4388.utility.status.FaultTalonFX; @@ -93,9 +83,9 @@ public class RobotMap { // Configure Shooter 22,23,24 - TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id); - TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id); - TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id); + // TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id); + // TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id); + // TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id); //Configure Intake 20,21 TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id); @@ -105,7 +95,9 @@ public class RobotMap { // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - shooterIO = new ShooterReal(shooter1, shooter2, indexer); + + shooterIO = new ShooterIO() {}; + // shooterIO = new ShooterReal(shooter1, shooter2, indexer); intakeIO = new IntakeReal(arm, roller); @@ -113,9 +105,9 @@ public class RobotMap { FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); - FaultTalonFX.addDevice(shooter1, "Shooter1"); - FaultTalonFX.addDevice(shooter2, "Shooter2"); - FaultTalonFX.addDevice(indexer, "Indexer"); + // FaultTalonFX.addDevice(shooter1, "Shooter1"); + // FaultTalonFX.addDevice(shooter2, "Shooter2"); + // FaultTalonFX.addDevice(indexer, "Indexer"); FaultTalonFX.addDevice(arm, "Arm"); FaultTalonFX.addDevice(roller, "Roller"); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index e49a662..0a393b8 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,13 +7,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 34; - public static final String GIT_SHA = "2d9ed527bebbf79d7643452fe4344ad32b3fab49"; - public static final String GIT_DATE = "2026-02-07 14:51:05 MST"; - public static final String GIT_BRANCH = "Subsystem-Boilerplate"; - public static final String BUILD_DATE = "2026-02-07 14:51:24 MST"; - public static final long BUILD_UNIX_TIME = 1770501084043L; - public static final int DIRTY = 0; + public static final int GIT_REVISION = 35; + public static final String GIT_SHA = "684e18ddcc5438f14f05c5902506d14cc4cbab19"; + public static final String GIT_DATE = "2026-02-07 14:52:21 MST"; + public static final String GIT_BRANCH = "arm-test"; + public static final String BUILD_DATE = "2026-02-09 16:43:56 MST"; + public static final long BUILD_UNIX_TIME = 1770680636562L; + public static final int DIRTY = 1; private BuildConstants(){} } diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index c2ecd18..4e87f34 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -33,11 +33,11 @@ public class Intake extends SubsystemBase { public void setMode(IntakeMode mode) { switch (mode) { case Up: - // io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER); + io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER); io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP); break; case Down: - // io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER); + io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER); io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY); break; } @@ -60,17 +60,11 @@ public class Intake extends SubsystemBase { @Override public void periodic() { - - - // FaultReporter.register(this); // TODO Implement fault reporter Logger.processInputs("Intake", state); - Pose2d pose = m_swervePoseSupplier.get(); - Angle robotRot = pose.getRotation().getMeasure(); - io.updateInputs(state); } diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 197a789..77570c1 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -17,14 +17,14 @@ import frc4388.utility.status.CanDevice; public class IntakeConstants { // Motor conversions - public static final double ARM_MOTOR_GEAR_RATIO = 1.; - public static final double ROLLER_MOTOR_GEAR_RATIO = 1.; + public static final double ARM_MOTOR_GEAR_RATIO = 1/100; + public static final double ROLLER_MOTOR_GEAR_RATIO = 1/3; //IDs - public static final CanDevice ARM_ID = new CanDevice("SHOOTER 1", 20); - public static final CanDevice ROLLER_ID = new CanDevice("SHOOTER 2", 21); + public static final CanDevice ARM_ID = new CanDevice("ARM", 20); + public static final CanDevice ROLLER_ID = new CanDevice("INTAKE_ROLLER", 21); // Limits @@ -44,7 +44,7 @@ public class IntakeConstants { .withKI(0.0) .withKD(10.0); - public static final Slot1Configs ROLLER_PID = new Slot1Configs() + public static final Slot0Configs ROLLER_PID = new Slot0Configs() .withKP(2.0) .withKI(0.0) .withKD(10.0); @@ -65,19 +65,6 @@ public class IntakeConstants { .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means ); - public static final class IDs { - public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22); - } - // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() - // .withCurrentLimits( - // new CurrentLimitsConfigs() - // .withStatorCurrentLimit(40) // TODO: tune??? - // .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means - // ).withMotorOutput( - // new MotorOutputConfigs() - // .withNeutralMode(NeutralModeValue.Brake) // Brake so it stop - // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means - // ); public static final TalonFXConfiguration ROLLER_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index c19d527..83ec7cf 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -20,6 +20,9 @@ public class IntakeReal implements IntakeIO { TalonFX m_armMotor; TalonFX m_rollerMotor; + PositionDutyCycle armPosition = new PositionDutyCycle(0); + VelocityDutyCycle rollerVelocity = new VelocityDutyCycle(0); + public IntakeReal( TalonFX armMotor, @@ -35,6 +38,9 @@ public class IntakeReal implements IntakeIO { m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG); m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_PID); m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG); + + armPosition.Slot = 0; + rollerVelocity.Slot = 0; } private Angle clampAng(Angle x, Angle min, Angle max){ @@ -53,9 +59,11 @@ public class IntakeReal implements IntakeIO { public void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) { state.rollerTargetVelocity = angularVelocity; // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC) - double motorSpeed = angularVelocity.in(RotationsPerSecond) / IntakeConstants.ROLLER_MOTOR_GEAR_RATIO; - VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed); - m_rollerMotor.setControl(velocity); + AngularVelocity motorSpeed = angularVelocity.div(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO); + + // m_rollerMotor.set(motorSpeed); + // VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed); + m_rollerMotor.setControl(rollerVelocity.withVelocity(motorSpeed)); } @Override @@ -63,26 +71,39 @@ public class IntakeReal implements IntakeIO { state.armTargetAngle = angle; // Assume that the angle is always accurate, because I think we will use a shaft encoder // Assume that 0 degrees = forwards. Might need an offset here - - Angle boundedAngle = clampAng(angle, IntakeConstants.ARM_LIMIT_LOWER, IntakeConstants.ARM_LIMIT_UPPER); + + // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT - double motorTargetAngle = boundedAngle.in(Rotations) / IntakeConstants.ARM_MOTOR_GEAR_RATIO; - PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); - m_armMotor.setControl(posRequest); + Angle motorAngle = angle.div(IntakeConstants.ARM_MOTOR_GEAR_RATIO); + + // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); + m_armMotor.setControl(armPosition.withPosition(motorAngle)); } + ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.2); + ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KP", 0); + ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KP", 0); + + ConfigurableDouble roller_kP = new ConfigurableDouble("Roller KP", 0.2); + ConfigurableDouble roller_kI = new ConfigurableDouble("Roller KI", 0); + ConfigurableDouble roller_kD = new ConfigurableDouble("Roller KD", 0); + @Override public void updateInputs(IntakeState state) { state.armAngle = m_armMotor.getPosition().getValue().times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); state.armMotorCurrent = m_armMotor.getStatorCurrent(false).getValue(); - // state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO); - // state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue(); - - // state.armAngle = m_armMotor.getPosition().getValue(); - // state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue(); - state.rollerVelocity = m_rollerMotor.getVelocity().getValue(); state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue(); + + IntakeConstants.ARM_PID.kP = arm_kP.get(); + IntakeConstants.ARM_PID.kI = arm_kI.get(); + IntakeConstants.ARM_PID.kD = arm_kD.get(); + m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG); + + IntakeConstants.ROLLER_PID.kP = roller_kP.get(); + IntakeConstants.ROLLER_PID.kI = roller_kI.get(); + IntakeConstants.ROLLER_PID.kD = roller_kD.get(); + m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG); } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 159c70b..0a39b1f 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -3,14 +3,11 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.InchesPerSecond; import static edu.wpi.first.units.Units.RotationsPerSecond; -import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.utility.configurable.ConfigurableDouble; public class ShooterReal implements ShooterIO { @@ -27,9 +24,9 @@ public class ShooterReal implements ShooterIO { TalonFX shooter2Motor, TalonFX indexerMotor ) { - m_shooter1Motor= shooter1Motor; - m_shooter2Motor= shooter2Motor; - m_indexerMotor = indexerMotor; + m_shooter1Motor = shooter1Motor; + m_shooter2Motor = shooter2Motor; + m_indexerMotor = indexerMotor; m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); m_indexerMotor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); From 51d2b80ea0ff88911192799340745aaa9338a413 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Mon, 9 Feb 2026 17:18:54 -0800 Subject: [PATCH 25/29] Robot should be ready --- .../java/frc4388/robot/RobotContainer.java | 53 ++++++++++++------ src/main/java/frc4388/robot/RobotMap.java | 23 ++++---- .../robot/constants/BuildConstants.java | 10 ++-- .../frc4388/robot/constants/Constants.java | 5 +- .../robot/subsystems/intake/Intake.java | 21 ++++--- .../subsystems/intake/IntakeConstants.java | 24 +++++--- .../robot/subsystems/intake/IntakeIO.java | 1 + .../robot/subsystems/intake/IntakeReal.java | 4 ++ .../robot/subsystems/shooter/Shooter.java | 24 ++++---- .../subsystems/shooter/ShooterConstants.java | 40 +++++++++----- .../robot/subsystems/shooter/ShooterIO.java | 9 ++- .../robot/subsystems/shooter/ShooterReal.java | 55 +++++++++++++++---- .../swerve/SwerveDriveConstants.java | 4 +- .../frc4388/utility/status/FaultReporter.java | 5 +- 14 files changed, 183 insertions(+), 95 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 70c894c..b322e41 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -34,7 +34,9 @@ import frc4388.robot.constants.FieldConstants; // Subsystems import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.intake.Intake; +import frc4388.robot.subsystems.intake.Intake.IntakeMode; import frc4388.robot.subsystems.shooter.Shooter; +import frc4388.robot.subsystems.shooter.Shooter.ShooterMode; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; @@ -98,13 +100,13 @@ public class RobotContainer { // new InstantCommand(() -> System.out.println(m_robotLED.getMode())) // ); - private Command RobotShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Active), m_robotShooter) - ); + // private Command RobotShoot = new SequentialCommandGroup( + // new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Active), m_robotShooter) + // ); - private Command RobotIntake = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Down), m_robotIntake) - ); + // private Command RobotIntake = new SequentialCommandGroup( + // new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Down), m_robotIntake) + // ); public RobotContainer() { @@ -147,23 +149,23 @@ public class RobotContainer { private void configureButtonBindings() { - new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) // .onTrue(new RotTo45(m_robotSwerveDrive)); - new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) - .onTrue(RobotShoot) - .onFalse(new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Resting), m_robotShooter)); + // new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) + // .onTrue(RobotShoot) + // .onFalse(new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Resting), m_robotShooter)); - new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode) - .onTrue(RobotIntake) - .onFalse(new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Up), m_robotShooter)); + // new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode) + // .onTrue(RobotIntake) + // .onFalse(new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Up), m_robotShooter)); - new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + // .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); @@ -172,12 +174,27 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Active);}, m_robotShooter)); + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Inactive);}, m_robotShooter)); + + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Extended);}, m_robotIntake)); + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Retracted);}, m_robotIntake)); + + // new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); - // new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.io.updateGains(); + m_robotShooter.io.updateGains(); + })); // IF the driver is holding the aim button, aim the robot towards the hub new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index ea8f489..3cc013a 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -11,6 +11,8 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.swerve.SwerveDrivetrain; + +import frc4388.robot.constants.Constants; //import frc4388.robot.constants.Constants.ElevatorConstants; import frc4388.robot.constants.Constants.SimConstants; import frc4388.robot.subsystems.intake.IntakeConstants; @@ -82,22 +84,21 @@ public class RobotMap { swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); // Configure Shooter 22,23,24 - - // TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id); - // TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id); - // TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id); + TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id, Constants.RIO_CANBUS); + TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id, Constants.RIO_CANBUS); + TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id, Constants.RIO_CANBUS); //Configure Intake 20,21 - TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id); - TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id); + TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id, Constants.RIO_CANBUS); + TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id, Constants.RIO_CANBUS); // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - shooterIO = new ShooterIO() {}; - // shooterIO = new ShooterReal(shooter1, shooter2, indexer); + // shooterIO = new ShooterIO() {}; + shooterIO = new ShooterReal(shooter1, shooter2, indexer); intakeIO = new IntakeReal(arm, roller); @@ -105,9 +106,9 @@ public class RobotMap { FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); - // FaultTalonFX.addDevice(shooter1, "Shooter1"); - // FaultTalonFX.addDevice(shooter2, "Shooter2"); - // FaultTalonFX.addDevice(indexer, "Indexer"); + FaultTalonFX.addDevice(shooter1, "Shooter1"); + FaultTalonFX.addDevice(shooter2, "Shooter2"); + FaultTalonFX.addDevice(indexer, "Indexer"); FaultTalonFX.addDevice(arm, "Arm"); FaultTalonFX.addDevice(roller, "Roller"); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 0a393b8..c3642e0 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 35; - public static final String GIT_SHA = "684e18ddcc5438f14f05c5902506d14cc4cbab19"; - public static final String GIT_DATE = "2026-02-07 14:52:21 MST"; + public static final int GIT_REVISION = 36; + public static final String GIT_SHA = "d90bddac0fff1c8f1bb6dd9a6b4a862c822a005a"; + public static final String GIT_DATE = "2026-02-09 17:03:48 MST"; public static final String GIT_BRANCH = "arm-test"; - public static final String BUILD_DATE = "2026-02-09 16:43:56 MST"; - public static final long BUILD_UNIX_TIME = 1770680636562L; + public static final String BUILD_DATE = "2026-02-09 18:14:36 MST"; + public static final long BUILD_UNIX_TIME = 1770686076395L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index 490aefd..4c461a8 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -7,6 +7,8 @@ package frc4388.robot.constants; +import com.ctre.phoenix6.CANBus; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; @@ -31,7 +33,8 @@ import frc4388.utility.structs.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { - public static final String CANBUS_NAME = "rio"; + public static final CANBus RIO_CANBUS = CANBus.roboRIO(); + public static final CANBus CANIVORE_CANBUS = new CANBus("drivetrain"); // public static final class LiDARConstants { // public static final int REEF_LIDAR_DIO_CHANNEL = 7; diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 4e87f34..0b212d7 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -1,6 +1,9 @@ package frc4388.robot.subsystems.intake; import static edu.wpi.first.units.Units.Rotation; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; import java.util.function.Supplier; @@ -12,7 +15,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.utility.status.FaultReporter; public class Intake extends SubsystemBase { - IntakeIO io; + public IntakeIO io; IntakeStateAutoLogged state = new IntakeStateAutoLogged(); Supplier m_swervePoseSupplier; @@ -26,19 +29,19 @@ public class Intake extends SubsystemBase { } public enum IntakeMode { - Up, - Down, + Extended, + Retracted, } public void setMode(IntakeMode mode) { switch (mode) { - case Up: - io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER); - io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP); + case Extended: + io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get())); + io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get())); break; - case Down: - io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER); - io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY); + case Retracted: + io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); + io.setRollerVelocity(state, RotationsPerSecond.of(0)); break; } } diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 77570c1..9d1fc48 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -12,6 +12,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.CanDevice; public class IntakeConstants { @@ -33,21 +34,28 @@ public class IntakeConstants { //when testing the negative output of 10% made the robot put the intake up - public static final Angle ARM_LIMIT_LOWER = Degrees.of(90); - public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90); - public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0); - public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0); + // public static final Angle ARM_LIMIT_LOWER = Degrees.of(90); + // public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90); + + public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0); + public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.25); + + public static final ConfigurableDouble ROLLER_ACTIVE = new ConfigurableDouble("Rolelr Active", 10); + // public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25); + + // public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0); + // public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0); public static final Slot0Configs ARM_PID = new Slot0Configs() - .withKP(2.0) + .withKP(0.2) .withKI(0.0) - .withKD(10.0); + .withKD(0.0); public static final Slot0Configs ROLLER_PID = new Slot0Configs() - .withKP(2.0) + .withKP(0.2) .withKI(0.0) - .withKD(10.0); + .withKD(0.0); // 0 is paralell to the ground, 90 is directly up // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java index 92ba274..b97ad76 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -39,4 +39,5 @@ public interface IntakeIO { public default void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {} public default void updateInputs(IntakeState state) {} + public default void updateGains() {} } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index 83ec7cf..ce89c55 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -95,6 +95,10 @@ public class IntakeReal implements IntakeIO { state.rollerVelocity = m_rollerMotor.getVelocity().getValue(); state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue(); + } + + @Override + public void updateGains() { IntakeConstants.ARM_PID.kP = arm_kP.get(); IntakeConstants.ARM_PID.kI = arm_kI.get(); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 2809bd2..488d634 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -1,5 +1,9 @@ package frc4388.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Rotation; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -11,7 +15,7 @@ import frc4388.robot.subsystems.intake.IntakeConstants; import frc4388.robot.subsystems.shooter.ShooterIO.ShooterState; public class Shooter extends SubsystemBase { - ShooterIO io; + public ShooterIO io; ShooterStateAutoLogged state = new ShooterStateAutoLogged(); // Supplier m_swervePoseSupplier; @@ -46,19 +50,19 @@ public class Shooter extends SubsystemBase { public void setMode(ShooterMode mode) { switch (mode) { case Active: - io.setMotor1Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY); - io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY); - io.setIndexerVelocity(state, ShooterConstants.INDEXER_ACTIVE_VELOCITY); + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); + // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY); + io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_ACTIVE_VELOCITY.get())); break; case Resting: - io.setMotor1Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setIndexerVelocity(state, ShooterConstants.INDEXER_INACTIVE_VELOCITY); + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); + // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); + io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_INACTIVE_VELOCITY.get())); break; case Inactive: - io.setMotor1Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setIndexerVelocity(state, ShooterConstants.INDEXER_INACTIVE_VELOCITY); + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); + // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); + io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_INACTIVE_VELOCITY.get())); break; } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 34c5b76..69b7f1a 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.CanDevice; public class ShooterConstants { @@ -20,16 +21,20 @@ public class ShooterConstants { public static final double SHOOTERMOTOR1_GEAR_RATIO = 1.; public static final double SHOOTERMOTOR2_GEAR_RATIO = 1.; public static final double INDEXER_GEAR_RATIO = 1.; - - public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(0.0); - public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(-15); - public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); - public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(0); - public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); - + // public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(30); + // public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(15); + // public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); + // public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(10); + // public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); + public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 30); + public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15); + public static final ConfigurableDouble SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); + + public static final ConfigurableDouble INDEXER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 10); + public static final ConfigurableDouble INDEXER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) @@ -37,6 +42,12 @@ public class ShooterConstants { .withKI(0.0) .withKD(0.0); + public static Slot0Configs INDEXER_PID = new Slot0Configs() + .withKV(0.0) + .withKP(0.0) + .withKI(0.0) + .withKD(0.0); + // Limits // 0 is the forward angle on the robot. @@ -45,8 +56,8 @@ public class ShooterConstants { // public static final Angle ANGLE_LIMIT_RIGHT = Degrees.of(180); // 0 is paralell to the ground, 90 is directly up - public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); - public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); + // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); + // public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); // Motor configs // public static final TalonFXConfiguration ANGLE_MOTOR_CONFIG = new TalonFXConfiguration() @@ -61,10 +72,10 @@ public class ShooterConstants { // ); - public static final CanDevice SHOOTER1_ID = new CanDevice("SHOOTER 1", 22); - public static final CanDevice SHOOTER2_ID = new CanDevice("SHOOTER 2", 23); - public static final CanDevice INDEXER_ID = new CanDevice("INDEXER",24); - + public static final CanDevice SHOOTER1_ID = new CanDevice("SHOOTER 1", 22); + public static final CanDevice SHOOTER2_ID = new CanDevice("SHOOTER 2", 23); + public static final CanDevice INDEXER_ID = new CanDevice("INDEXER",24); + public static final TalonFXConfiguration SHOOTER1_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( @@ -73,7 +84,7 @@ public class ShooterConstants { .withStatorCurrentLimitEnable(true) ).withMotorOutput( new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means ); public static final TalonFXConfiguration SHOOTER2_MOTOR_CONFIG = new TalonFXConfiguration() @@ -86,6 +97,7 @@ public class ShooterConstants { .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means ); + public static final TalonFXConfiguration INDEXER_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 5f0a1b8..2e6402b 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -36,14 +36,17 @@ public interface ShooterIO { Current motor2Current = Amps.of(0); Current indexerCurrent = Amps.of(0); - LinearVelocity motorLinearVelocity = InchesPerSecond.of(0); + // LinearVelocity motorLinearVelocity = InchesPerSecond.of(0); } // public default void setShooterAngle(ShooterState state, Angle angle) {} // public default void setShooterPitch(ShooterState state, Angle angle) {} - public default void setMotor1Velocity(ShooterState state, AngularVelocity angularVelocity) {} - public default void setMotor2Velocity(ShooterState state, AngularVelocity linearVelocity) {} + public default void setShooterVelocity(ShooterState state, AngularVelocity angularVelocity) {} + // public default void setMotor2Velocity(ShooterState state, AngularVelocity angularVelocity) {} public default void setIndexerVelocity(ShooterState state, AngularVelocity linearVelocity) {} public default void updateInputs(ShooterState state) {} + + + public default void updateGains() {} } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 0a39b1f..4d1b2c9 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -8,12 +8,15 @@ import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import frc4388.robot.subsystems.intake.IntakeConstants; +import frc4388.utility.configurable.ConfigurableDouble; public class ShooterReal implements ShooterIO { TalonFX m_shooter1Motor; TalonFX m_shooter2Motor; TalonFX m_indexerMotor; + VelocityDutyCycle shooter1Velocity = new VelocityDutyCycle(0); VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0); VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0); @@ -27,14 +30,18 @@ public class ShooterReal implements ShooterIO { m_shooter1Motor = shooter1Motor; m_shooter2Motor = shooter2Motor; m_indexerMotor = indexerMotor; + m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); m_indexerMotor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); - m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER1_MOTOR_CONFIG); m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER2_MOTOR_CONFIG); m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_MOTOR_CONFIG); + + shooter1Velocity.Slot = 0; + shooter2Velocity.Slot = 0; + m_indexerVelocity.Slot = 0; } private Angle clampAng(Angle x, Angle min, Angle max){ @@ -83,34 +90,60 @@ public class ShooterReal implements ShooterIO { @Override - public void setMotor1Velocity(ShooterState state, AngularVelocity target) { + public void setShooterVelocity(ShooterState state, AngularVelocity target) { state.motor1TargetVelocity = target; state.motor2TargetVelocity = target; - double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO; - m_shooter1Motor.setControl(new VelocityDutyCycle(motorRps)); - m_shooter2Motor.setControl(new VelocityDutyCycle(motorRps)); + AngularVelocity motorRps = target.div(ShooterConstants.INDEXER_GEAR_RATIO); + + m_shooter1Motor.setControl(shooter1Velocity.withVelocity(motorRps)); + m_shooter2Motor.setControl(shooter2Velocity.withVelocity(motorRps)); } @Override public void setIndexerVelocity(ShooterState state, AngularVelocity target) { state.indexerTargetVelocity = target; - double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO; - m_indexerMotor.setControl(new VelocityDutyCycle(motorRps)); + AngularVelocity motorRps = target.div(ShooterConstants.INDEXER_GEAR_RATIO); + m_indexerMotor.setControl(m_indexerVelocity.withVelocity(motorRps)); } + + ConfigurableDouble indexer_kP = new ConfigurableDouble("Indexer KP", 0.2); + ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KP", 0); + ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KP", 0); + + ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.2); + ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0); + ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); + @Override public void updateInputs(ShooterState state) { - state.motor1Velocity = m_shooter1Motor.getVelocity().getValue(); - state.motor2Velocity = m_shooter2Motor.getVelocity().getValue(); - state.indexerVelocity = m_indexerMotor.getVelocity().getValue(); + state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().times(ShooterConstants.SHOOTERMOTOR1_GEAR_RATIO); + state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().times(ShooterConstants.SHOOTERMOTOR2_GEAR_RATIO); + state.indexerVelocity = m_indexerMotor.getVelocity().getValue().times(ShooterConstants.INDEXER_GEAR_RATIO); - state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT); + // state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT); state.motor1Current = m_shooter1Motor.getStatorCurrent().getValue(); state.motor2Current = m_shooter2Motor.getStatorCurrent().getValue(); state.indexerCurrent = m_indexerMotor.getStatorCurrent().getValue(); + + } + + @Override + public void updateGains() { + // TEMPORARY PIDs + ShooterConstants.SHOOTER_PID.kP = shooter_kP.get(); + ShooterConstants.SHOOTER_PID.kI = shooter_kI.get(); + ShooterConstants.SHOOTER_PID.kD = shooter_kD.get(); + m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); + m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); + + ShooterConstants.INDEXER_PID.kP = indexer_kP.get(); + ShooterConstants.INDEXER_PID.kI = indexer_kI.get(); + ShooterConstants.INDEXER_PID.kD = indexer_kD.get(); + m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_PID); } } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index af2f6fc..4e1139f 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -22,6 +22,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Angle; +import frc4388.robot.constants.Constants; import frc4388.utility.configurable.ConfigurableDouble; //import edu.wpi.first.units.measure.measure.Distance; import frc4388.utility.status.CanDevice; @@ -31,7 +32,6 @@ import frc4388.utility.structs.Gains; // No mans land // Beware, there be dragons. public final class SwerveDriveConstants { - public static final String CANBUS_NAME = "drivetrain"; public static final double MAX_ROT_SPEED = Math.PI * 2; public static final double AUTO_MAX_ROT_SPEED = 1.5; public static final double MIN_ROT_SPEED = 1.0; @@ -209,7 +209,7 @@ public final class SwerveDriveConstants { } public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withPigeon2Id(IDs.DRIVE_PIGEON.id).withCANBusName(CANBUS_NAME); + .withPigeon2Id(IDs.DRIVE_PIGEON.id).withCANBusName(Constants.CANIVORE_CANBUS.getName()); private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() // holy verbosity batman. diff --git a/src/main/java/frc4388/utility/status/FaultReporter.java b/src/main/java/frc4388/utility/status/FaultReporter.java index afd4dd4..f4066f8 100644 --- a/src/main/java/frc4388/utility/status/FaultReporter.java +++ b/src/main/java/frc4388/utility/status/FaultReporter.java @@ -85,13 +85,12 @@ public class FaultReporter { } } - // CAN header System.out.println(CAN_HEADER); - CANBus canBus = new CANBus(Constants.CANBUS_NAME); + // CANBus canBus = new CANBus(); - CANBusStatus canInfo = canBus.getStatus(); + CANBusStatus canInfo = Constants.RIO_CANBUS.getStatus(); System.out.println("CANInfo BusOffCount - " + canInfo.BusOffCount); System.out.println("CANInfo BusUtilization - " + canInfo.BusUtilization); From 6ce6d0eb0b8faec5af448ec23ffca156303cbed5 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Mon, 9 Feb 2026 18:38:55 -0800 Subject: [PATCH 26/29] Get IDK to work --- .../subsystems/intake/IntakeConstants.java | 9 ++++++ .../robot/subsystems/intake/IntakeReal.java | 26 ++++++++--------- .../robot/subsystems/shooter/Shooter.java | 12 ++------ .../subsystems/shooter/ShooterConstants.java | 13 +++++++-- .../robot/subsystems/shooter/ShooterReal.java | 28 +++++++++---------- 5 files changed, 48 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 9d1fc48..f3c8aad 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -57,6 +57,15 @@ public class IntakeConstants { .withKI(0.0) .withKD(0.0); + public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.2); + public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KP", 0); + public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KP", 0); + + public static ConfigurableDouble roller_kP = new ConfigurableDouble("Roller KP", 0.2); + public static ConfigurableDouble roller_kI = new ConfigurableDouble("Roller KI", 0); + public static ConfigurableDouble roller_kD = new ConfigurableDouble("Roller KD", 0); + + // 0 is paralell to the ground, 90 is directly up // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); // public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index ce89c55..ea9c61a 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -58,6 +58,12 @@ public class IntakeReal implements IntakeIO { @Override public void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) { state.rollerTargetVelocity = angularVelocity; + + if(angularVelocity.baseUnitMagnitude() == 0) { + m_rollerMotor.set(0); + return; + } + // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC) AngularVelocity motorSpeed = angularVelocity.div(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO); @@ -80,14 +86,6 @@ public class IntakeReal implements IntakeIO { m_armMotor.setControl(armPosition.withPosition(motorAngle)); } - ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.2); - ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KP", 0); - ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KP", 0); - - ConfigurableDouble roller_kP = new ConfigurableDouble("Roller KP", 0.2); - ConfigurableDouble roller_kI = new ConfigurableDouble("Roller KI", 0); - ConfigurableDouble roller_kD = new ConfigurableDouble("Roller KD", 0); - @Override public void updateInputs(IntakeState state) { state.armAngle = m_armMotor.getPosition().getValue().times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); @@ -100,14 +98,14 @@ public class IntakeReal implements IntakeIO { @Override public void updateGains() { - IntakeConstants.ARM_PID.kP = arm_kP.get(); - IntakeConstants.ARM_PID.kI = arm_kI.get(); - IntakeConstants.ARM_PID.kD = arm_kD.get(); + IntakeConstants.ARM_PID.kP = IntakeConstants.arm_kP.get(); + IntakeConstants.ARM_PID.kI = IntakeConstants.arm_kI.get(); + IntakeConstants.ARM_PID.kD = IntakeConstants.arm_kD.get(); m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG); - IntakeConstants.ROLLER_PID.kP = roller_kP.get(); - IntakeConstants.ROLLER_PID.kI = roller_kI.get(); - IntakeConstants.ROLLER_PID.kD = roller_kD.get(); + IntakeConstants.ROLLER_PID.kP = IntakeConstants.roller_kP.get(); + IntakeConstants.ROLLER_PID.kI = IntakeConstants.roller_kI.get(); + IntakeConstants.ROLLER_PID.kD = IntakeConstants.roller_kD.get(); m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG); } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 488d634..869ed14 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -1,18 +1,11 @@ package frc4388.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.Rotation; -import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; -import java.util.function.Supplier; - import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.subsystems.intake.IntakeConstants; -import frc4388.robot.subsystems.shooter.ShooterIO.ShooterState; public class Shooter extends SubsystemBase { public ShooterIO io; @@ -47,6 +40,7 @@ public class Shooter extends SubsystemBase { Inactive, } + public void setMode(ShooterMode mode) { switch (mode) { case Active: @@ -57,12 +51,12 @@ public class Shooter extends SubsystemBase { case Resting: io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_INACTIVE_VELOCITY.get())); + io.setIndexerVelocity(state, RotationsPerSecond.of(0)); break; case Inactive: io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_INACTIVE_VELOCITY.get())); + io.setIndexerVelocity(state, RotationsPerSecond.of(0)); break; } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 69b7f1a..f398491 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -31,10 +31,10 @@ public class ShooterConstants { public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 30); public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15); - public static final ConfigurableDouble SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); + // public static final ConfigurableDouble SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); public static final ConfigurableDouble INDEXER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 10); - public static final ConfigurableDouble INDEXER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); + // public static final ConfigurableDouble INDEXER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) @@ -47,6 +47,15 @@ public class ShooterConstants { .withKP(0.0) .withKI(0.0) .withKD(0.0); + + + public static ConfigurableDouble indexer_kP = new ConfigurableDouble("Indexer KP", 0.2); + public static ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KP", 0); + public static ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KP", 0); + + public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.2); + public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0); + public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); // Limits diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 4d1b2c9..8aee915 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -94,6 +94,12 @@ public class ShooterReal implements ShooterIO { state.motor1TargetVelocity = target; state.motor2TargetVelocity = target; + if(target.baseUnitMagnitude() == 0) { + m_shooter1Motor.set(0); + m_shooter2Motor.set(0); + return; + } + AngularVelocity motorRps = target.div(ShooterConstants.INDEXER_GEAR_RATIO); m_shooter1Motor.setControl(shooter1Velocity.withVelocity(motorRps)); @@ -107,14 +113,6 @@ public class ShooterReal implements ShooterIO { m_indexerMotor.setControl(m_indexerVelocity.withVelocity(motorRps)); } - - ConfigurableDouble indexer_kP = new ConfigurableDouble("Indexer KP", 0.2); - ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KP", 0); - ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KP", 0); - - ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.2); - ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0); - ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); @Override public void updateInputs(ShooterState state) { @@ -130,19 +128,19 @@ public class ShooterReal implements ShooterIO { state.indexerCurrent = m_indexerMotor.getStatorCurrent().getValue(); } - + @Override public void updateGains() { // TEMPORARY PIDs - ShooterConstants.SHOOTER_PID.kP = shooter_kP.get(); - ShooterConstants.SHOOTER_PID.kI = shooter_kI.get(); - ShooterConstants.SHOOTER_PID.kD = shooter_kD.get(); + ShooterConstants.SHOOTER_PID.kP = ShooterConstants.shooter_kP.get(); + ShooterConstants.SHOOTER_PID.kI = ShooterConstants.shooter_kI.get(); + ShooterConstants.SHOOTER_PID.kD = ShooterConstants.shooter_kD.get(); m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); - ShooterConstants.INDEXER_PID.kP = indexer_kP.get(); - ShooterConstants.INDEXER_PID.kI = indexer_kI.get(); - ShooterConstants.INDEXER_PID.kD = indexer_kD.get(); + ShooterConstants.INDEXER_PID.kP = ShooterConstants.indexer_kP.get(); + ShooterConstants.INDEXER_PID.kI = ShooterConstants.indexer_kI.get(); + ShooterConstants.INDEXER_PID.kD = ShooterConstants.indexer_kD.get(); m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_PID); } From 539c1bd8eb96c91eb22dfc882b8a16591d4d0de2 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Tue, 10 Feb 2026 17:33:39 -0800 Subject: [PATCH 27/29] Working robot in testing --- src/main/java/frc4388/robot/RobotMap.java | 10 ++--- .../robot/constants/BuildConstants.java | 10 ++--- .../robot/subsystems/intake/Intake.java | 24 +++++++----- .../subsystems/intake/IntakeConstants.java | 12 +++--- .../robot/subsystems/intake/IntakeReal.java | 14 +++---- .../robot/subsystems/shooter/Shooter.java | 37 ++++++++++--------- .../subsystems/shooter/ShooterConstants.java | 12 +++--- .../robot/subsystems/shooter/ShooterReal.java | 16 +++++--- 8 files changed, 74 insertions(+), 61 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 3cc013a..5067e69 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -84,13 +84,13 @@ public class RobotMap { swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); // Configure Shooter 22,23,24 - TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id, Constants.RIO_CANBUS); - TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id, Constants.RIO_CANBUS); - TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id, Constants.RIO_CANBUS); + TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id, Constants.CANIVORE_CANBUS); + TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id, Constants.CANIVORE_CANBUS); + TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id, Constants.CANIVORE_CANBUS); //Configure Intake 20,21 - TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id, Constants.RIO_CANBUS); - TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id, Constants.RIO_CANBUS); + TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id, Constants.CANIVORE_CANBUS); + TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id, Constants.CANIVORE_CANBUS); // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index c3642e0..764bf0b 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 36; - public static final String GIT_SHA = "d90bddac0fff1c8f1bb6dd9a6b4a862c822a005a"; - public static final String GIT_DATE = "2026-02-09 17:03:48 MST"; + public static final int GIT_REVISION = 38; + public static final String GIT_SHA = "6ce6d0eb0b8faec5af448ec23ffca156303cbed5"; + public static final String GIT_DATE = "2026-02-09 19:38:55 MST"; public static final String GIT_BRANCH = "arm-test"; - public static final String BUILD_DATE = "2026-02-09 18:14:36 MST"; - public static final long BUILD_UNIX_TIME = 1770686076395L; + public static final String BUILD_DATE = "2026-02-10 17:12:07 MST"; + public static final long BUILD_UNIX_TIME = 1770768727439L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 0b212d7..d83292a 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -33,17 +33,10 @@ public class Intake extends SubsystemBase { Retracted, } + private IntakeMode mode = IntakeMode.Extended; + public void setMode(IntakeMode mode) { - switch (mode) { - case Extended: - io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get())); - io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get())); - break; - case Retracted: - io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); - io.setRollerVelocity(state, RotationsPerSecond.of(0)); - break; - } + this.mode = mode; } @@ -70,6 +63,17 @@ public class Intake extends SubsystemBase { io.updateInputs(state); + switch (mode) { + case Extended: + io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get())); + io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get())); + break; + case Retracted: + io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); + io.setRollerVelocity(state, RotationsPerSecond.of(0)); + break; + } + } } diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index f3c8aad..f2c3490 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -18,8 +18,8 @@ import frc4388.utility.status.CanDevice; public class IntakeConstants { // Motor conversions - public static final double ARM_MOTOR_GEAR_RATIO = 1/100; - public static final double ROLLER_MOTOR_GEAR_RATIO = 1/3; + public static final double ARM_MOTOR_GEAR_RATIO = 100; + public static final double ROLLER_MOTOR_GEAR_RATIO = 3; //IDs @@ -37,8 +37,8 @@ public class IntakeConstants { // public static final Angle ARM_LIMIT_LOWER = Degrees.of(90); // public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90); - public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0); - public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.25); + public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", -0.3); + public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0); public static final ConfigurableDouble ROLLER_ACTIVE = new ConfigurableDouble("Rolelr Active", 10); // public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25); @@ -58,8 +58,8 @@ public class IntakeConstants { .withKD(0.0); public static ConfigurableDouble arm_kP = new ConfigurableDouble("ARM KP", 0.2); - public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KP", 0); - public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KP", 0); + public static ConfigurableDouble arm_kI = new ConfigurableDouble("ARM KI", 0); + public static ConfigurableDouble arm_kD = new ConfigurableDouble("ARM KD", 0); public static ConfigurableDouble roller_kP = new ConfigurableDouble("Roller KP", 0.2); public static ConfigurableDouble roller_kI = new ConfigurableDouble("Roller KI", 0); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index ea9c61a..598fa91 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -65,7 +65,7 @@ public class IntakeReal implements IntakeIO { } // (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC) - AngularVelocity motorSpeed = angularVelocity.div(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO); + AngularVelocity motorSpeed = angularVelocity.times(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO); // m_rollerMotor.set(motorSpeed); // VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed); @@ -80,7 +80,7 @@ public class IntakeReal implements IntakeIO { // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT - Angle motorAngle = angle.div(IntakeConstants.ARM_MOTOR_GEAR_RATIO); + Angle motorAngle = angle.times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); m_armMotor.setControl(armPosition.withPosition(motorAngle)); @@ -88,10 +88,10 @@ public class IntakeReal implements IntakeIO { @Override public void updateInputs(IntakeState state) { - state.armAngle = m_armMotor.getPosition().getValue().times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); - state.armMotorCurrent = m_armMotor.getStatorCurrent(false).getValue(); + state.armAngle = m_armMotor.getPosition().getValue().div(IntakeConstants.ARM_MOTOR_GEAR_RATIO); + state.armMotorCurrent = m_armMotor.getStatorCurrent().getValue(); - state.rollerVelocity = m_rollerMotor.getVelocity().getValue(); + state.rollerVelocity = m_rollerMotor.getVelocity().getValue().div(IntakeConstants.ROLLER_MOTOR_GEAR_RATIO); state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue(); } @@ -101,11 +101,11 @@ public class IntakeReal implements IntakeIO { IntakeConstants.ARM_PID.kP = IntakeConstants.arm_kP.get(); IntakeConstants.ARM_PID.kI = IntakeConstants.arm_kI.get(); IntakeConstants.ARM_PID.kD = IntakeConstants.arm_kD.get(); - m_armMotor.getConfigurator().apply(IntakeConstants.ARM_MOTOR_CONFIG); + m_armMotor.getConfigurator().apply(IntakeConstants.ARM_PID); IntakeConstants.ROLLER_PID.kP = IntakeConstants.roller_kP.get(); IntakeConstants.ROLLER_PID.kI = IntakeConstants.roller_kI.get(); IntakeConstants.ROLLER_PID.kD = IntakeConstants.roller_kD.get(); - m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG); + m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_PID); } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 869ed14..49ab73e 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -40,25 +40,10 @@ public class Shooter extends SubsystemBase { Inactive, } + private ShooterMode mode = ShooterMode.Inactive; public void setMode(ShooterMode mode) { - switch (mode) { - case Active: - io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); - // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY); - io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_ACTIVE_VELOCITY.get())); - break; - case Resting: - io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); - // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setIndexerVelocity(state, RotationsPerSecond.of(0)); - break; - case Inactive: - io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); - // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setIndexerVelocity(state, RotationsPerSecond.of(0)); - break; - } + this.mode = mode; } // Calculate what should be done based off of the position of the robot @@ -83,5 +68,23 @@ public class Shooter extends SubsystemBase { io.updateInputs(state); + switch (mode) { + case Active: + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); + // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY); + io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_ACTIVE_VELOCITY.get())); + break; + case Resting: + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); + // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); + io.setIndexerVelocity(state, RotationsPerSecond.of(0)); + break; + case Inactive: + io.setShooterVelocity(state, RotationsPerSecond.of(0)); + // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); + io.setIndexerVelocity(state, RotationsPerSecond.of(0)); + break; + } + } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index f398491..81deb0e 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -14,7 +14,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.CanDevice; -public class ShooterConstants { +public class ShooterConstants { // Motor conversions public static final double FEEDER_INCHES_PER_ROT = 1.; @@ -33,25 +33,25 @@ public class ShooterConstants { public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15); // public static final ConfigurableDouble SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); - public static final ConfigurableDouble INDEXER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 10); + public static final ConfigurableDouble INDEXER_ACTIVE_VELOCITY = new ConfigurableDouble("Indexer Active Velocity", 10); // public static final ConfigurableDouble INDEXER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) .withKP(0.0) .withKI(0.0) - .withKD(0.0); + .withKD(0.2); public static Slot0Configs INDEXER_PID = new Slot0Configs() .withKV(0.0) .withKP(0.0) .withKI(0.0) - .withKD(0.0); + .withKD(0.2); public static ConfigurableDouble indexer_kP = new ConfigurableDouble("Indexer KP", 0.2); - public static ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KP", 0); - public static ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KP", 0); + public static ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KI", 0); + public static ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KD", 0); public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.2); public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 8aee915..928b09d 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -100,7 +100,7 @@ public class ShooterReal implements ShooterIO { return; } - AngularVelocity motorRps = target.div(ShooterConstants.INDEXER_GEAR_RATIO); + AngularVelocity motorRps = target.times(ShooterConstants.INDEXER_GEAR_RATIO); m_shooter1Motor.setControl(shooter1Velocity.withVelocity(motorRps)); m_shooter2Motor.setControl(shooter2Velocity.withVelocity(motorRps)); @@ -109,7 +109,13 @@ public class ShooterReal implements ShooterIO { @Override public void setIndexerVelocity(ShooterState state, AngularVelocity target) { state.indexerTargetVelocity = target; - AngularVelocity motorRps = target.div(ShooterConstants.INDEXER_GEAR_RATIO); + + if(target.baseUnitMagnitude() == 0) { + m_indexerMotor.set(0); + return; + } + + AngularVelocity motorRps = target.times(ShooterConstants.INDEXER_GEAR_RATIO); m_indexerMotor.setControl(m_indexerVelocity.withVelocity(motorRps)); } @@ -117,9 +123,9 @@ public class ShooterReal implements ShooterIO { @Override public void updateInputs(ShooterState state) { - state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().times(ShooterConstants.SHOOTERMOTOR1_GEAR_RATIO); - state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().times(ShooterConstants.SHOOTERMOTOR2_GEAR_RATIO); - state.indexerVelocity = m_indexerMotor.getVelocity().getValue().times(ShooterConstants.INDEXER_GEAR_RATIO); + state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR1_GEAR_RATIO); + state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR2_GEAR_RATIO); + state.indexerVelocity = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO); // state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT); From 983b95fdc704ef35b6f5e2dada2c6348f3c67190 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Tue, 10 Feb 2026 18:42:47 -0800 Subject: [PATCH 28/29] IDk --- .../java/frc4388/robot/RobotContainer.java | 61 +++++++----- .../robot/subsystems/intake/Intake.java | 4 + .../robot/subsystems/intake/IntakeReal.java | 1 + .../robot/subsystems/shooter/Shooter.java | 93 ++++++++++++------- .../subsystems/shooter/ShooterConstants.java | 17 +++- .../robot/subsystems/shooter/ShooterReal.java | 45 --------- 6 files changed, 118 insertions(+), 103 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b322e41..2682829 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -65,7 +65,7 @@ public class RobotContainer { public final Vision m_vision = new Vision(); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO); - public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO); + public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED); private Boolean operatorManualMode = false; // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); @@ -175,15 +175,15 @@ public class RobotContainer { - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Active);}, m_robotShooter)); - new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Inactive);}, m_robotShooter)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + // .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Active);}, m_robotShooter)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + // .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Inactive);}, m_robotShooter)); - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Extended);}, m_robotIntake)); - new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Retracted);}, m_robotIntake)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> {;}, m_robotIntake)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Retracted);}, m_robotIntake)); @@ -196,18 +196,7 @@ public class RobotContainer { m_robotShooter.io.updateGains(); })); - // IF the driver is holding the aim button, aim the robot towards the hub - new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) - .whileTrue(new RunCommand( - () -> { - m_robotSwerveDrive.driveFacingPosition( - getDeadbandedDriverController().getLeft(), - FieldConstants.BLUE_HUB_POS); - }, m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); - - - // IF the driver is holding the aim button, aim the robot towards the hub + // IF the driver is holding the left trigger, intake driving new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) .whileTrue(new RunCommand( () -> { @@ -216,7 +205,35 @@ public class RobotContainer { false ); }, m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); + // .onFalse(new InstantCommand(() -> , m_robotSwerveDrive)); + + + .onTrue(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Extended); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Retracted); + m_robotSwerveDrive.softStop(); + }, m_robotSwerveDrive)); + + // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready + new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) + .whileTrue(new RunCommand( + () -> { + m_robotSwerveDrive.driveFacingPosition( + getDeadbandedDriverController().getLeft(), + FieldConstants.BLUE_HUB_POS); + }, m_robotSwerveDrive) + ) + .onTrue(new InstantCommand(() -> { + // When Right trigger is pressed, + m_robotIntake.setMode(IntakeMode.Extended); + })) + .onFalse(new InstantCommand(() -> { + m_robotIntake.setMode(IntakeMode.Retracted); + m_robotSwerveDrive.softStop(); + }, m_robotSwerveDrive)); + // D-PAD fine alignment new Trigger(() -> getDeadbandedDriverController().getPOV() != -1) diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index d83292a..be507b3 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -39,6 +39,10 @@ public class Intake extends SubsystemBase { this.mode = mode; } + public IntakeMode getMode() { + return mode; + } + // public enum FieldZone { // // The robot should aim at the hub diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index 598fa91..547cc90 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -78,6 +78,7 @@ public class IntakeReal implements IntakeIO { // Assume that the angle is always accurate, because I think we will use a shaft encoder // Assume that 0 degrees = forwards. Might need an offset here + // angle = clampAng(angle, IntakeConstants.ARM_LIMIT_RETRACTED, IntakeConstants.ARM_LIMIT_EXTENDED); // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT Angle motorAngle = angle.times(IntakeConstants.ARM_MOTOR_GEAR_RATIO); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 49ab73e..35e2501 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -2,23 +2,38 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.RotationsPerSecond; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.intake.Intake; +import frc4388.robot.subsystems.intake.Intake.IntakeMode; +import frc4388.robot.subsystems.swerve.SwerveDrive; public class Shooter extends SubsystemBase { public ShooterIO io; ShooterStateAutoLogged state = new ShooterStateAutoLogged(); + SwerveDrive m_SwerveDrive; + Intake m_Intake; + LED m_robotLED; + + // Supplier m_swervePoseSupplier; public Shooter( - ShooterIO io - // Supplier swervePoseSupplier + ShooterIO io, + SwerveDrive swerveDrive, + Intake intake, + LED robotLED ) { this.io = io; + this.m_SwerveDrive = swerveDrive; + this.m_Intake = intake; + this.m_robotLED = robotLED; // this.m_swervePoseSupplier = swervePoseSupplier; } @@ -32,57 +47,67 @@ public class Shooter extends SubsystemBase { public enum ShooterMode { - //Shooter is at speed it fires balls - Active, - //Shooter is at a resting velocity - Resting, - //Shooter is inactive (Off) - Inactive, + // Shooter is actively shooting + Shooting, + // Shooter is going to fire soon + Ready, + // Not ready to shoot + NotReady, } - private ShooterMode mode = ShooterMode.Inactive; + private ShooterMode mode = ShooterMode.NotReady; - public void setMode(ShooterMode mode) { - this.mode = mode; + // public void setMode(ShooterMode mode) { + // this.mode = mode; + // } + + public void setShooterReady() { + if(this.mode == ShooterMode.NotReady) { + this.mode = ShooterMode.Ready; + } + } + + public void setShooterNotReady() { + this.mode = ShooterMode.NotReady; } - // Calculate what should be done based off of the position of the robot - // TODO: Implement field zones - public FieldZone getTarget(Pose2d position) { - return FieldZone.InShootZone; + @AutoLogOutput + public ShooterMode getMode() { + return mode; } + @Override public void periodic() { - - - // FaultReporter.register(this); // TODO Implement fault reporter - Logger.processInputs("Shooter", state); - - // Pose2d pose = m_swervePoseSupplier.get(); - // Angle robotRot = pose.getRotation().getMeasure(); - io.updateInputs(state); + if(this.mode != ShooterMode.NotReady) { + double badRobotSpeed = Math.sqrt(Math.pow(m_SwerveDrive.chassisSpeeds.vxMetersPerSecond,2) + Math.pow(m_SwerveDrive.chassisSpeeds.vyMetersPerSecond,2)); + double badAngSpeed = Math.abs(m_SwerveDrive.chassisSpeeds.omegaRadiansPerSecond); + double badShooterVelocity = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2; + boolean intakeBad = m_Intake.getMode() == IntakeMode.Extended; + + + + // TODO: get if the robot is within the correct distance of the hub + } + switch (mode) { - case Active: + case Shooting: io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); - // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY); - io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_ACTIVE_VELOCITY.get())); + io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_FORWARD_VELOCITY.get())); break; - case Resting: + case Ready: + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); + io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_VELOCITY.get())); + break; + case NotReady: io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); - // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setIndexerVelocity(state, RotationsPerSecond.of(0)); - break; - case Inactive: - io.setShooterVelocity(state, RotationsPerSecond.of(0)); - // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setIndexerVelocity(state, RotationsPerSecond.of(0)); + io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_VELOCITY.get())); break; } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 81deb0e..531ed12 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -33,8 +33,21 @@ public class ShooterConstants { public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15); // public static final ConfigurableDouble SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); - public static final ConfigurableDouble INDEXER_ACTIVE_VELOCITY = new ConfigurableDouble("Indexer Active Velocity", 10); - // public static final ConfigurableDouble INDEXER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); + public static final ConfigurableDouble INDEXER_FORWARD_VELOCITY = new ConfigurableDouble("Indexer FWD Velocity", 10); + public static final ConfigurableDouble INDEXER_REVERSE_VELOCITY = new ConfigurableDouble("Indexer reverse Velocity", 10); + + // Tolerances + public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist", 1); + public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist", 1); + + public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Shoot Ang tolerance", 1); + + public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Shoot speed tolerance", 1); + public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance", 1); + + public static final ConfigurableDouble SHOOT_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance", 1); + + public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 928b09d..34dc2df 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -43,52 +43,7 @@ public class ShooterReal implements ShooterIO { shooter2Velocity.Slot = 0; m_indexerVelocity.Slot = 0; } - - private Angle clampAng(Angle x, Angle min, Angle max){ - if(x.gt(max)) { - return max; - }else if(x.lt(min)) { - return min; - }else{ - return x; - } - } - - - - - - // // TODO: Test - // @Override - // public void setShooterAngle(ShooterState state, Angle angle) { - // state.shooterTargetAngle = angle; - // // Assume that the angle is always accurate, because I think we will use a shaft encoder - // // Assume that 0 degrees = forwards. Might need an offset here - - // Angle boundedAngle = clampAng(angle, ShooterConstants.ANGLE_LIMIT_LEFT, ShooterConstants.ANGLE_LIMIT_RIGHT); - // // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT - // double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.ANGLE_MOTOR_GEAR_RATIO; - // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); - // m_angleMotor.setControl(posRequest); - // } - - - // TODO: Test - // @Override - // public void setShooterPitch(ShooterState state, Angle angle) { - // state.shooterTargetPitch = angle; - // // TODO: Test - // // This assumes that the 0 is paralell to the ground. Might need an offset here - - - // Angle boundedAngle = clampAng(angle, ShooterConstants.PITCH_LIMIT_UPPER, ShooterConstants.PITCH_LIMIT_LOWER); - // // (REAL_ROT) * (MOTOR_ROT / REAL_ROT) = MOTOR_ROT - // double motorTargetAngle = boundedAngle.in(Rotations) / ShooterConstants.PITCH_MOTOR_GEAR_RATIO; - // PositionDutyCycle posRequest = new PositionDutyCycle(motorTargetAngle); - // m_pitchMotor.setControl(posRequest); - // } - @Override public void setShooterVelocity(ShooterState state, AngularVelocity target) { state.motor1TargetVelocity = target; From f2415195ce9462f3232fba3bac086875545f48ca Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Wed, 11 Feb 2026 15:18:12 -0700 Subject: [PATCH 29/29] Implement field positions, tolerance, LED states --- .../java/frc4388/robot/RobotContainer.java | 18 +++-- .../robot/constants/BuildConstants.java | 16 ++--- .../frc4388/robot/constants/Constants.java | 19 +++-- .../robot/subsystems/climber/Climber.java | 27 ++------ .../robot/subsystems/shooter/Shooter.java | 69 ++++++++++++++++--- .../subsystems/shooter/ShooterConstants.java | 14 ++-- .../utility/compute/FieldPositions.java | 21 ++++++ 7 files changed, 125 insertions(+), 59 deletions(-) create mode 100644 src/main/java/frc4388/utility/compute/FieldPositions.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2682829..9a6b70e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -40,6 +40,7 @@ import frc4388.robot.subsystems.shooter.Shooter.ShooterMode; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; +import frc4388.utility.compute.FieldPositions; import frc4388.utility.compute.TimesNegativeOne; import frc4388.utility.controller.DeadbandedXboxController; // Autos @@ -112,24 +113,29 @@ public class RobotContainer { configureButtonBindings(); - DeferredBlock.addBlock(() -> { // Called on first robot enable + // Called on first robot enable + DeferredBlock.addBlock(() -> { m_robotSwerveDrive.resetGyro(); }, false); - DeferredBlock.addBlock(() -> { // Called on every robot enable + + // Called on every robot enable + DeferredBlock.addBlock(() -> { TimesNegativeOne.update(); + FieldPositions.update(); }, true); DriverStation.silenceJoystickConnectionWarning(true); + // Drive normally m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - // Drive normally m_robotSwerveDrive.driveWithInput( getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getRight(),true); }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); + m_robotSwerveDrive.setToSlow(); makeAutoChooser(); @@ -196,6 +202,7 @@ public class RobotContainer { m_robotShooter.io.updateGains(); })); + // IF the driver is holding the left trigger, intake driving new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) .whileTrue(new RunCommand( @@ -205,9 +212,6 @@ public class RobotContainer { false ); }, m_robotSwerveDrive)) - // .onFalse(new InstantCommand(() -> , m_robotSwerveDrive)); - - .onTrue(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Extended); })) @@ -222,7 +226,7 @@ public class RobotContainer { () -> { m_robotSwerveDrive.driveFacingPosition( getDeadbandedDriverController().getLeft(), - FieldConstants.BLUE_HUB_POS); + FieldPositions.HUB_POSITION); }, m_robotSwerveDrive) ) .onTrue(new InstantCommand(() -> { diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 764bf0b..abf86f9 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -5,15 +5,15 @@ package frc4388.robot.constants; */ public final class BuildConstants { public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "2026KPopRobotHunters"; + public static final String MAVEN_NAME = "2026KPopRobotHunters-new"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 38; - public static final String GIT_SHA = "6ce6d0eb0b8faec5af448ec23ffca156303cbed5"; - public static final String GIT_DATE = "2026-02-09 19:38:55 MST"; - public static final String GIT_BRANCH = "arm-test"; - public static final String BUILD_DATE = "2026-02-10 17:12:07 MST"; - public static final long BUILD_UNIX_TIME = 1770768727439L; - public static final int DIRTY = 1; + public static final int GIT_REVISION = 40; + public static final String GIT_SHA = "983b95fdc704ef35b6f5e2dada2c6348f3c67190"; + public static final String GIT_DATE = "2026-02-10 19:42:47 MST"; + public static final String GIT_BRANCH = "shoot-button"; + public static final String BUILD_DATE = "2026-02-11 12:41:33 MST"; + public static final long BUILD_UNIX_TIME = 1770838893524L; + public static final int DIRTY = 0; private BuildConstants(){} } diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index 4c461a8..9ff5234 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -98,13 +98,20 @@ public final class Constants { public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; - public static final LEDPatterns WAITING_PATTERN = LEDPatterns.SOLID_RED; - public static final LEDPatterns DOWN_PATTERN = LEDPatterns.SOLID_YELLOW; - public static final LEDPatterns READY_PATTERN = LEDPatterns.SOLID_GREEN_DARK; - public static final LEDPatterns SCORING_PATTERN = LEDPatterns.RAINBOW_RAINBOW; + // LED color for when the intake is out + public static final LEDPatterns INTAKE_OUT = LEDPatterns.SOLID_RED; + // LED color for when the intake is out, but the driver conditions are bad + public static final LEDPatterns INTAKE_OUT_BADPHYS = LEDPatterns.RED_STROBE; - public static final LEDPatterns RED_PATTERN = LEDPatterns.LAVA_WAVES; - public static final LEDPatterns BLUE_PATTERN = LEDPatterns.OCEAN_WAVES; + // LED color for when the flywheel speed isn't in tolarance + public static final LEDPatterns BAD_FLYWEEL = LEDPatterns.SOLID_GOLD; + // LED color for when the flywheel speed isn't in tolarance, but the driver conditions are bad + public static final LEDPatterns BAD_FLYWEEL_BADPHYS = LEDPatterns.GOLD_STROBE; + + // Operator ready to shoot + public static final LEDPatterns OPREADY = LEDPatterns.SOLID_WHITE; + // Operator ready to shoot, but the driver conditions are bad + public static final LEDPatterns OPREADY_BADPHYS = LEDPatterns.WHITE_STROBE; } public static final class OIConstants { diff --git a/src/main/java/frc4388/robot/subsystems/climber/Climber.java b/src/main/java/frc4388/robot/subsystems/climber/Climber.java index f667f1b..78c06eb 100644 --- a/src/main/java/frc4388/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/climber/Climber.java @@ -1,7 +1,5 @@ package frc4388.robot.subsystems.climber; -import static edu.wpi.first.units.Units.Rotation; - import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -9,20 +7,19 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.utility.status.FaultReporter; public class Climber extends SubsystemBase { ClimberIO io; ClimberStateAutoLogged state = new ClimberStateAutoLogged(); - Supplier m_swervePoseSupplier; + // Supplier m_swervePoseSupplier; public Climber( - ClimberIO io, - Supplier swervePoseSupplier + ClimberIO io + // Supplier swervePoseSupplier ) { this.io = io; - this.m_swervePoseSupplier = swervePoseSupplier; + // this.m_swervePoseSupplier = swervePoseSupplier; } // public enum ClimberMode { @@ -44,19 +41,6 @@ public class Climber extends SubsystemBase { // } - // public enum FieldZone { - // // The robot should aim at the hub - // InShootZone, - // // The robot should aim towards the wall - // AimAtWall, - - // } - - // // Calculate what should be done based off of the position of the robot - // // TODO: Implement field zones - // public FieldZone getTarget(Pose2d position) { - // return FieldZone.InShootZone; - // } @Override public void periodic() { @@ -68,9 +52,6 @@ public class Climber extends SubsystemBase { Logger.processInputs("Climber", state); - Pose2d pose = m_swervePoseSupplier.get(); - Angle robotRot = pose.getRotation().getMeasure(); - io.updateInputs(state); } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 35e2501..44c9b2c 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -2,15 +2,21 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.RotationsPerSecond; +import java.text.FieldPosition; + import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.constants.Constants; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.intake.Intake; import frc4388.robot.subsystems.intake.Intake.IntakeMode; import frc4388.robot.subsystems.swerve.SwerveDrive; +import frc4388.utility.compute.FieldPositions; public class Shooter extends SubsystemBase { public ShooterIO io; @@ -57,10 +63,6 @@ public class Shooter extends SubsystemBase { private ShooterMode mode = ShooterMode.NotReady; - // public void setMode(ShooterMode mode) { - // this.mode = mode; - // } - public void setShooterReady() { if(this.mode == ShooterMode.NotReady) { this.mode = ShooterMode.Ready; @@ -86,14 +88,65 @@ public class Shooter extends SubsystemBase { io.updateInputs(state); if(this.mode != ShooterMode.NotReady) { - double badRobotSpeed = Math.sqrt(Math.pow(m_SwerveDrive.chassisSpeeds.vxMetersPerSecond,2) + Math.pow(m_SwerveDrive.chassisSpeeds.vyMetersPerSecond,2)); - double badAngSpeed = Math.abs(m_SwerveDrive.chassisSpeeds.omegaRadiansPerSecond); - double badShooterVelocity = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2; + + ChassisSpeeds speed = m_SwerveDrive.chassisSpeeds; + double XYSpeed = Math.sqrt(Math.pow(speed.vxMetersPerSecond,2) + Math.pow(speed.vyMetersPerSecond,2)); + double AngSpeed = Math.abs(speed.omegaRadiansPerSecond * (180/Math.PI)); + + Pose2d robotPose2d = m_SwerveDrive.getPose2d(); + + double distanceToHub = robotPose2d.getTranslation().minus(FieldPositions.HUB_POSITION).getNorm(); + + + // TODO: get if the robot is within the angle of the hub + + boolean driverError = + XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() | + AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() | + distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() | + distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get(); + + double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2; + + boolean badShooterVelocity = shooterSpeed < ShooterConstants.SHOOTER_SPEED_TOLERANCE.get(); boolean intakeBad = m_Intake.getMode() == IntakeMode.Extended; + + int bitmask = (driverError ? 1 : 0) + (badShooterVelocity ? 2 : 0) + (intakeBad ? 4 : 0); + switch (bitmask) { + case 0b000: // No Errors + m_robotLED.setMode(Constants.LEDConstants.OPREADY); + break; + case 0b001: // No op err, yes driver err + m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS); + break; + case 0b010: // Bad flywheel, no driver err + m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL); + break; + case 0b011: // Bad flywheel, yes driver err + m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS); + break; + case 0b100: // Bad intake, no driver err + m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT); + break; + case 0b101: // Bad intake, yes driver err + m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS); + break; + case 0b110: // Bad intake and shooter (intake is more important), no driver err + m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT); + break; + case 0b111: // Bad intake and shooter (intake is more important), yes driver err + m_robotLED.setMode(Constants.LEDConstants.INTAKE_OUT_BADPHYS); + break; + } + // We set the shooter mode to ready if there are no errors + mode = ( + bitmask == 0 ? + ShooterMode.Ready : + ShooterMode.NotReady + ); - // TODO: get if the robot is within the correct distance of the hub } switch (mode) { diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 531ed12..108ef6e 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -31,21 +31,21 @@ public class ShooterConstants { public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 30); public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15); - // public static final ConfigurableDouble SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); public static final ConfigurableDouble INDEXER_FORWARD_VELOCITY = new ConfigurableDouble("Indexer FWD Velocity", 10); public static final ConfigurableDouble INDEXER_REVERSE_VELOCITY = new ConfigurableDouble("Indexer reverse Velocity", 10); + // Tolerances - public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist", 1); - public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist", 1); + public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 0); + public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 99); - public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Shoot Ang tolerance", 1); + public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360); - public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Shoot speed tolerance", 1); - public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance", 1); + public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1); + public static final ConfigurableDouble ROBOT_ANG_SPEED_TOLERANCE = new ConfigurableDouble("Shoot Ang speed tolerance DEG", 3); - public static final ConfigurableDouble SHOOT_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance", 1); + public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 1); diff --git a/src/main/java/frc4388/utility/compute/FieldPositions.java b/src/main/java/frc4388/utility/compute/FieldPositions.java new file mode 100644 index 0000000..19457d8 --- /dev/null +++ b/src/main/java/frc4388/utility/compute/FieldPositions.java @@ -0,0 +1,21 @@ +package frc4388.utility.compute; + +import edu.wpi.first.math.geometry.Translation2d; + +public class FieldPositions { + public static final Translation2d RED_HUB_POSITION = new Translation2d(0, 0); + public static final Translation2d BLUE_HUB_POSITION = new Translation2d(0, 0); + + // We set the default position to one just in case it doesn't update + // Setting to null could cause a null pointer, and setting to (0,0) could cause problems + // I would rather have the 50/50 chance than a code error + public static Translation2d HUB_POSITION = RED_HUB_POSITION; + + public static void update() { + if(TimesNegativeOne.isRed) { + HUB_POSITION = RED_HUB_POSITION; + } else { + HUB_POSITION = BLUE_HUB_POSITION; + } + } +}