From 787121122a1d783c659d010c59ec65a7790bb594 Mon Sep 17 00:00:00 2001 From: paragrghoshal Date: Mon, 2 Feb 2026 19:33:37 -0500 Subject: [PATCH 01/12] new changes --- src/main/java/frc/robot/Constants.java | 7 ++- src/main/java/frc/robot/Robot.java | 15 +++++- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/controls/DriverController.java | 51 +++++++++++++++++++ .../frc/robot/subsystems/LimelightSystem.java | 47 +++++++++-------- .../frc/robot/subsystems/SwerveSystem.java | 3 +- .../frc/robot/subsystems/TurretSystem.java | 5 ++ 7 files changed, 106 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4130dbe..ac36f23 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,11 +1,16 @@ package frc.robot; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.util.Units; public class Constants { public static class SwerveDriveConstants { - public static final SwerveDriveKinematics k_kinematics = new SwerveDriveKinematics(); //get this in when we know bot dims + private static final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381); + private static final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381); + private static final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381); + private static final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381); + public static final SwerveDriveKinematics k_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); //get this in when we know bot dims public static final double k_maxSpeed = Units.feetToMeters(14.5); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cea1ffc..8e95c76 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,10 +7,20 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Subsystem; +import frc.robot.subsystems.SwerveSystem; +import frc.robot.subsystems.LimelightSystem; +import frc.robot.subsystems.TurretSystem; public class Robot extends TimedRobot { private Command m_autonomousCommand; + private SwerveSystem m_swerve; + private LimelightSystem m_limelight; + private TurretSystem m_TurretSystem; + + private Subsystem[] m_subsystems = {m_swerve, m_limelight, m_TurretSystem}; + private final RobotContainer m_robotContainer; public Robot() { @@ -19,7 +29,10 @@ public Robot() { @Override public void robotPeriodic() { - CommandScheduler.getInstance().run(); + for (Subsystem system : m_subsystems){ + system.periodic(); + } + CommandScheduler.getInstance().run(); //how is it going } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8138600..463b999 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,7 +11,7 @@ import frc.robot.subsystems.SwerveSystem; public class RobotContainer { - private final SwerveSystem m_swerve = new SwerveSystem();; + private final SwerveSystem m_swerve = new SwerveSystem(); public RobotContainer() { configureBindings(); diff --git a/src/main/java/frc/robot/controls/DriverController.java b/src/main/java/frc/robot/controls/DriverController.java index 3ee9fbe..e49aa34 100644 --- a/src/main/java/frc/robot/controls/DriverController.java +++ b/src/main/java/frc/robot/controls/DriverController.java @@ -1,5 +1,6 @@ package frc.robot.controls; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.SwerveSystem; import swervelib.SwerveInputStream; @@ -26,4 +27,54 @@ public static void configure(int port, SwerveSystem drivetrain) { public static CommandXboxController getController(){ return controller; } + + public static void testGetControllerButtonA(){ + controller.a().onTrue(Commands.runOnce(() -> { + System.out.println("Button A Pressed"); + })); + } + + public static void testGetControllerButtonB(){ + controller.b().onTrue(Commands.runOnce(() -> { + System.out.println("Button B Pressed"); + })); + } + + public static void testGetControllerButtonX(){ + controller.x().onTrue(Commands.runOnce(() -> { + System.out.println("Button X Pressed"); + + })); + } + public static void testGetControllerButtonY(){ + controller.y().onTrue(Commands.runOnce(() -> { + System.out.println("Button Y Pressed"); + })); + } + + public static void testGetControllerTriggerRight(){ + controller.rightTrigger().onTrue(Commands.runOnce(() -> { + System.out.println("Trigger Right Pressed"); + })); + } + + public static void testGetControllerTriggerLeft(){ + controller.leftTrigger().onTrue(Commands.runOnce(() -> { + System.out.println("Trigger Left Pressed"); + })); + } + + public static void testGetControllerBumperRight(){ + controller.rightBumper().onTrue(Commands.runOnce(() -> { + System.out.println("Bumper Right Pressed"); + + })); + } + + public static void testGetControllerBumperLeft(){ + controller.leftBumper().onTrue(Commands.runOnce(() -> { + System.out.println("Bumper Left Pressed"); + })); + } } + //why are we here? \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/LimelightSystem.java b/src/main/java/frc/robot/subsystems/LimelightSystem.java index e3f8f23..b6edad9 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSystem.java +++ b/src/main/java/frc/robot/subsystems/LimelightSystem.java @@ -11,6 +11,7 @@ import limelight.networktables.LimelightSettings.LEDMode; import limelight.networktables.Orientation3d; import limelight.networktables.PoseEstimate; +import limelight.networktables.LimelightSettings.ImuMode; import swervelib.SwerveDrive; import frc.robot.Constants; @@ -35,25 +36,26 @@ public LimelightSystem(SwerveDrive swerve){ swerveDrive = swerve; limelight.getSettings() - .withLimelightLEDMode(LEDMode.PipelineControl) - .withCameraOffset(Pose3d.kZero) - .withRobotOrientation( - new Orientation3d(swerveDrive.getGyro().getRotation3d(), - new AngularVelocity3d( - DegreesPerSecond.of(0), - DegreesPerSecond.of(0), - DegreesPerSecond.of(0)))) - .save(); + .withLimelightLEDMode(LEDMode.PipelineControl) + .withCameraOffset(Pose3d.kZero) + .withRobotOrientation( + new Orientation3d(swerveDrive.getGyro().getRotation3d(), + new AngularVelocity3d( + DegreesPerSecond.of(0), + DegreesPerSecond.of(0), + DegreesPerSecond.of(0)))) + .withImuMode(ImuMode.InternalImuExternalAssist) + .save(); visionEstimate = limelight.createPoseEstimator(EstimationMode.MEGATAG2).getPoseEstimate(); } @Override - public void periodic(){ + public void periodic() { //if the pose is there visionEstimate.ifPresent((PoseEstimate poseEstimate) -> { - this.allowed = this.exceptions(poseEstimate); - if (this.allowed) { + this.allowed = this.rejectUpdate(poseEstimate); + if (!this.allowed) { swerveDrive.addVisionMeasurement( poseEstimate.pose.toPose2d(), poseEstimate.timestampSeconds); @@ -61,21 +63,26 @@ public void periodic(){ }); } - public void onEnabled(){ + public void onEnabled() { limelight.getSettings() // .withImuMode(ImuMode.InternalImuMT1Assist) .withImuAssistAlpha(0.01) .save(); } - public boolean exceptions(PoseEstimate foo) { - if (foo.tagCount <= 0) { return false; } - if (foo.pose.getX() <= 0 || foo.pose.getX() > Constants.FieldConstants.k_length) { return false; } - if (foo.pose.getY() <= 0 || foo.pose.getY() > Constants.FieldConstants.k_width) { return false;} - if (Math.abs(swerveDrive.getRobotVelocity().vxMetersPerSecond) > 720) { return false; } - if (Math.abs(swerveDrive.getRobotVelocity().vyMetersPerSecond) > 720) { return false; } + public boolean rejectUpdate(PoseEstimate foo) { + /* + returns true if Pose didn't pass tests + returns false if Pose passed tests + */ + if (foo.tagCount <= 0) { return true; } + if (foo.getAvgTagAmbiguity() > 0.7 ) { return true; } + if (foo.pose.getX() <= 0 || foo.pose.getX() > Constants.FieldConstants.k_length) { return true; } + if (foo.pose.getY() <= 0 || foo.pose.getY() > Constants.FieldConstants.k_width) { return true;} + if (Math.abs(swerveDrive.getRobotVelocity().omegaRadiansPerSecond) > Math.PI * 2) { return true; } + if (Math.abs(swerveDrive.getRobotVelocity().omegaRadiansPerSecond) > Math.PI * 2) { return true; } - // TODO make sure the april tag area is legibi + // TODO make sure the april tag area is legible return true; diff --git a/src/main/java/frc/robot/subsystems/SwerveSystem.java b/src/main/java/frc/robot/subsystems/SwerveSystem.java index 2b88066..2d56a1b 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSystem.java @@ -21,6 +21,7 @@ import swervelib.SwerveDrive; import swervelib.SwerveModule; import frc.robot.Telemetry; +import frc.robot.controls.DriverController; import swervelib.SwerveInputStream; public class SwerveSystem extends SubsystemBase { @@ -74,7 +75,7 @@ public SwerveSystem() { @Override public void periodic(){ - m_limelight.periodic(); + DriverController.testGetControllerButtonA(); } @Override diff --git a/src/main/java/frc/robot/subsystems/TurretSystem.java b/src/main/java/frc/robot/subsystems/TurretSystem.java index 5e04073..a72bf99 100644 --- a/src/main/java/frc/robot/subsystems/TurretSystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSystem.java @@ -11,6 +11,11 @@ public TurretSystem() { } + @Override + public void periodic(){ + + } + public static double calculateLaunchVelocity(double launchAngle, double xDisplacement, double yDisplacement) { double cos = Math.cos(Math.toRadians(launchAngle)); double sin = Math.sin(Math.toRadians(launchAngle)); From f28351d34f21d027747f8a30c42873a2768ffe1b Mon Sep 17 00:00:00 2001 From: = <=> Date: Wed, 4 Feb 2026 20:56:40 -0500 Subject: [PATCH 02/12] refactored variables, made code look cleaner --- .vscode/launch.json | 12 ++++++++--- build.gradle | 2 +- src/main/java/frc/robot/Robot.java | 2 +- .../frc/robot/subsystems/LimelightSystem.java | 21 +++++++++---------- .../frc/robot/subsystems/SwerveSystem.java | 2 +- 5 files changed, 22 insertions(+), 17 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index c9c9713..41297e8 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,18 +4,24 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ - + { + "type": "java", + "name": "Main", + "request": "launch", + "mainClass": "frc.robot.Main", + "projectName": "RA26_RobotCode" + }, { "type": "wpilib", "name": "WPILib Desktop Debug", "request": "launch", - "desktop": true, + "desktop": true }, { "type": "wpilib", "name": "WPILib roboRIO Debug", "request": "launch", - "desktop": false, + "desktop": false } ] } diff --git a/build.gradle b/build.gradle index f2ceadb..21805c5 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2026.1.1" + id "edu.wpi.first.GradleRIO" version "2026.2.1" } java { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8e95c76..6582897 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -17,7 +17,7 @@ public class Robot extends TimedRobot { private SwerveSystem m_swerve; private LimelightSystem m_limelight; - private TurretSystem m_TurretSystem; + private TurretSystem m_TurretSystem; private Subsystem[] m_subsystems = {m_swerve, m_limelight, m_TurretSystem}; diff --git a/src/main/java/frc/robot/subsystems/LimelightSystem.java b/src/main/java/frc/robot/subsystems/LimelightSystem.java index b6edad9..8dd2d23 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSystem.java +++ b/src/main/java/frc/robot/subsystems/LimelightSystem.java @@ -52,7 +52,7 @@ public LimelightSystem(SwerveDrive swerve){ @Override public void periodic() { - //if the pose is there + // if the pose is there visionEstimate.ifPresent((PoseEstimate poseEstimate) -> { this.allowed = this.rejectUpdate(poseEstimate); if (!this.allowed) { @@ -69,22 +69,21 @@ public void onEnabled() { .withImuAssistAlpha(0.01) .save(); } - - public boolean rejectUpdate(PoseEstimate foo) { + /* + + */ + public boolean rejectUpdate(PoseEstimate poseEstimate) { /* returns true if Pose didn't pass tests returns false if Pose passed tests */ - if (foo.tagCount <= 0) { return true; } - if (foo.getAvgTagAmbiguity() > 0.7 ) { return true; } - if (foo.pose.getX() <= 0 || foo.pose.getX() > Constants.FieldConstants.k_length) { return true; } - if (foo.pose.getY() <= 0 || foo.pose.getY() > Constants.FieldConstants.k_width) { return true;} + + if (poseEstimate.getAvgTagAmbiguity() > 0.7 ) { return true; } + if (poseEstimate.pose.getX() <= 0 || poseEstimate.pose.getX() > Constants.FieldConstants.k_length) { return true; } + if (poseEstimate.pose.getY() <= 0 || poseEstimate.pose.getY() > Constants.FieldConstants.k_width) { return true; } if (Math.abs(swerveDrive.getRobotVelocity().omegaRadiansPerSecond) > Math.PI * 2) { return true; } if (Math.abs(swerveDrive.getRobotVelocity().omegaRadiansPerSecond) > Math.PI * 2) { return true; } - // TODO make sure the april tag area is legible - - return true; - + return false; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveSystem.java b/src/main/java/frc/robot/subsystems/SwerveSystem.java index 2d56a1b..5b5c011 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSystem.java @@ -39,7 +39,7 @@ public SwerveSystem() { this.m_kinematics = Constants.SwerveDriveConstants.k_kinematics; this.m_limelight = new LimelightSystem(swerveDrive); - File swerveDir = new File(Filesystem.getDeployDirectory(), "swerve"); + File swerveDir = new File(Filesystem.getDeployDirectory(), "swerve/modules"); try { this.parser = new SwerveParser(swerveDir); From 1cd51773e505961386e51d803f3fd5062e9ae395 Mon Sep 17 00:00:00 2001 From: = <=> Date: Mon, 9 Feb 2026 18:49:01 -0500 Subject: [PATCH 03/12] config update, checkout to branch to retrieve new code --- .../roborioteamnumbersetter.json | 3 ++ vendordeps/AdvantageKit.json | 35 +++++++++++++++++++ 2 files changed, 38 insertions(+) create mode 100644 .roboRIOTeamNumberSetter/roborioteamnumbersetter.json create mode 100644 vendordeps/AdvantageKit.json diff --git a/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json b/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json new file mode 100644 index 0000000..b70849a --- /dev/null +++ b/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json @@ -0,0 +1,3 @@ +{ + "TeamNumber": 1741 +} diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json new file mode 100644 index 0000000..162ad66 --- /dev/null +++ b/vendordeps/AdvantageKit.json @@ -0,0 +1,35 @@ +{ + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "26.0.0", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2026", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "26.0.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "26.0.0", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file From be6aadaf7c053e8d35faedf564d4740b60afd9b1 Mon Sep 17 00:00:00 2001 From: The-Candyman <147785024+The-Candyman@users.noreply.github.com> Date: Mon, 19 Jan 2026 05:07:56 +0000 Subject: [PATCH 04/12] Big turret math drop Lots of physics-based helper functions for the turret auto-aiming system. --- .../frc/robot/subsystems/TurretSystem.java | 129 +++++++++++++++--- 1 file changed, 107 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/TurretSystem.java b/src/main/java/frc/robot/subsystems/TurretSystem.java index a72bf99..d78a0c3 100644 --- a/src/main/java/frc/robot/subsystems/TurretSystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSystem.java @@ -1,8 +1,14 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; - +import frc.robot.Constants.TurretConstants.k_gravitationalAcceleration; +import frc.robot.Constants.TurretConstants.k_turretHeight; +import frc.robot.Constants.TurretConstants.k_extraTimeToPassSensor; +import frc.robot.Constants.FieldConstants.k_hubRadius; +import frc.robot.Constants.FieldConstants.k_hubHeight; +import frc.robot.Constants.FieldConstants.k_hubX; +import frc.robot.Constants.FieldConstants.k_hubY; +import frc.robot.Constants.FieldConstants.k_fuelRadius; public class TurretSystem extends SubsystemBase { @@ -11,24 +17,103 @@ public TurretSystem() { } - @Override - public void periodic(){ - - } + // TODO add desmos graph link - public static double calculateLaunchVelocity(double launchAngle, double xDisplacement, double yDisplacement) { - double cos = Math.cos(Math.toRadians(launchAngle)); - double sin = Math.sin(Math.toRadians(launchAngle)); - double divisor = 2.0 * cos * (xDisplacement * sin - yDisplacement * cos); - double radicand = Constants.TurretConstants.k_gravitationalAcceleration / divisor; - return xDisplacement * Math.sqrt(radicand); - } - public static double calculateLaunchAngle(double launchVelocity, double xDisplacement, double yDisplacement) { - return 0.0; - } - public static double calculateMaxHeightFromLaunch(double launchAngle, double launchVelocity) { - double vYSquared = Math.pow(launchVelocity * Math.sin(Math.toRadians(launchAngle)), 2.0); - double twoG = 2.0 * Constants.TurretConstants.k_gravitationalAcceleration; - return vYSquared / twoG; - } -} \ No newline at end of file + // launch geometry and calculations + + public static double launchVelocity(double launchAngle, double xDistance, double yDistance) { + double divisor = 2.0 * cos(launchAngle) * (xDistance * sin(launchAngle) - yDistance * cos(launchAngle)); + double radicand = k_gravitationalAcceleration / divisor; + return xDistance * Math.sqrt(radicand); + } + public static double upperLaunchAngle(double launchVelocity, double xDistance, double yDistance) { + double distance = Math.hypot(xDistance, yDistance); + double targetFactor = yDistance / distance; + double dropFactor = k_gravitationalAcceleration * xDistance * xDistance / (launchVelocity * launchVelocity * distance); + return 0.5 * (arccos(targetFactor + dropFactor) + arccos(-targetFactor)); + } + public static double lowerLaunchAngle(double launchVelocity, double xDistance, double yDistance) { + double distance = Math.hypot(xDistance, yDistance); + double targetFactor = yDistance / distance; + double dropFactor = k_gravitationalAcceleration * xDistance * xDistance / (launchVelocity * launchVelocity * distance); + return 0.5 * (arcsin(targetFactor + dropFactor) + arcsin(targetFactor)); + } + public static double maxLaunchHeight(double launchAngle, double launchVelocity) { + double vYSquared = Math.pow(launchVelocity * sin(launchAngle), 2.0); + return 0.5 * vYSquared / k_gravitationalAcceleration; + } + public static double absHeightAtDistance(double launchAngle, double launchVelocity, double testX) { + double time = testX / (launchVelocity * cos(launchAngle)); + double launchFactor = time * launchVelocity * sin(launchAngle); + double dropFactor = 0.5 * time * k_gravitationalAcceleration * k_gravitationalAcceleration; + return k_turretHeight + launchFactor - dropFactor; + } + public static double secondTimeToAbsHeight(double launchAngle, double launchVelocity, double testHeight) { + double VxSin = launchVelocity * sin(launchAngle); + double radicand = VxSin * VxSin - 2 * k_gravitationalAcceleration * (testHeight - k_turretHeight); + return (VxSin + Math.sqrt(radicand)) / k_gravitationalAcceleration; + } + public static double farDistanceToAbsHeight(double launchAngle, double launchVelocity, double testHeight) { + return launchVelocity * cos(launchAngle) * secondTimeToAbsoluteHeight(launchAngle, launchVelocity, testHeight); + } + public static double trajectorySlopeAtDistance(double launchAngle, double launchVelocity, double testX) { + double xVelocity = launchVelocity * cos(launchAngle); + double time = testX / xVelocity; + double yVelocity = launchVelocity * sin(launchAngle) - time * k_gravitationalAcceleration; + return yVelocity / xVelocity; + } + public static boolean testValidHubTrajectory(double launchDirection, double launchAngle, double launchVelocity, + double turretX, double turretY, double sideClearance, double verticalClearance) { + // turret x, y, height should be measured as the location of the bottom of the fuel at the moment it is launched from the turret + // weather permitting, air resistance may be neglected + double distance = Math.hypot(k_hubX - turretX, k_hubY - turretY); + double sideOffset = Math.abs(cos(launchDirection) * (k_hubY - turretY) - sin(launchDirection) * (k_hubX - turretX)); + double launchDistance = Math.sqrt(distance * distance - sideOffset * sideOffset); + double nearEdgeDistance = launchDistance + hubNearEdgeOffset(launchDirection, sideOffset, 0.0); + double entranceDistance = farDistanceToAbsoluteHeight(launchAngle, launchVelocity, k_hubHeight); + double entranceHeight = absHeightAtDistance(launchAngle, launchVelocity, nearEdgeDistance); + double maxHeight = 2.0 * k_fuelRadius + maxLaunchHeight(launchAngle, launchVelocity); + double nearClearanceDistance = launchDistance - hubNearEdgeOffset(launchDirection, sideOffset, sideClearance); + double farClearanceingDistance = launchDistance + hubFarEdgeOffset(launchDirection, sideOffset, sideClearance); + boolean clearsHub = entranceHeight >= verticalClearance // measured at the center of the fuel entering the space above the funnel + && entranceDistance >= nearClearanceDistance // measured at the bottom of the fuel hitting 72 inches above the ground + && entranceDistance <= farClearanceDistance // clearance treats the funnel opening as if it has been shrunk + && maxLaunchHeight <= k_ceilingHeight; // it is admittedly unlikely that the turret will hit the ceiling, but still + return clearsHub; + } + public static double timeToHubScoring(double launchAngle, double launchVelocity) { + return k_extraTimeToPassSensor + secondTimeToAbsHeight(launchAngle, launchVelocity, k_hubHeight); + } + + // field geometry and calculations + + public static double hubNearEdgeOffset(double approachAngle, double sideOffset, double clearance) { + return hubFarEdgeOffset(approachAngle, -sideOffset, clearance); // Exploits the hub's hexagonal symmetry + } // Hexagons really are the bestagons... + public static double hubFarEdgeOffset(double approachAngle, double sideOffset, double clearance) { + double hubRadius = k_hubRadius - clearance * 2.0 / Math.sqrt(3.0); + double[] pXArr = new double[4]; + pXArr[0] = hubRadius * cos(210 - approachAngle % 60); + pXArr[1] = hubRadius * cos(150 - approachAngle % 60); + pXArr[2] = hubRadius * cos(90 - approachAngle % 60); + pXArr[3] = hubRadius * cos(30 - approachAngle % 60); + int intersectingSide = + (sideOffset < pXArr[0] || sideOffset > pXArr[3])? NAN : + ((sideOffset < pXArr[1])? 0 : + ((sideOffset < pXArr[2])? 1 : 2)); + double p1X = pXArr[intersectingSide]; + double p2X = pXArr[intersectingSide + 1]; + double p1Y = Math.sqrt(hubRadius * hubRadius - p1X * p1X); + double p2Y = Math.sqrt(hubRadius * hubRadius - p2X * p2X); + double p1Scale = (sideOffset - p2X) / (p1X - p2X); + double p2Scale = (sideOffset - p1X) / (p2X - p1X); + return p1Y * p1Scale + p2Y * p2Scale; + } + + // math helpers + + public static double sin(double degrees) {return Math.sin(Math.toRadians(degrees));} + public static double cos(double degrees) {return Math.cos(Math.toRadians(degrees));} + public static double arcsin(double ratio) {return Math.toDegrees(Math.asin(ratio));} + public static double arccos(double ratio) {return Math.toDegrees(Math.acos(ratio));} +} From 3b25bd60af2556c8a1b74b3df2d1cab3eb76dba7 Mon Sep 17 00:00:00 2001 From: The-Candyman <147785024+The-Candyman@users.noreply.github.com> Date: Mon, 19 Jan 2026 06:05:57 +0000 Subject: [PATCH 05/12] Constants adjusted Adjusted constants to match imports for TurretSystem commit. Also created new FieldConstants section for mostly field dimensions. --- src/main/java/frc/robot/Constants.java | 27 +++++++++++++------------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ac36f23..2992c61 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,19 +25,18 @@ public static class ControllerConstants{ public static class TurretConstants { // feet (NOT INCHES), seconds, degrees, pounds (mass), pound*ft/s^2 (force) public static final double k_gravitationalAcceleration = 32.174; - public static final double k_turretHeight = 2.0; - public static final double k_hubHeight = 6.0; - public static final double k_ceilingHeight = 15.0; - public static final double k_fuelRadius = 0.246063; - public static final double k_fuelMass = 0.474; // estimate - public static final double k_minYHeightToHub = 0.5 + k_fuelRadius + k_hubHeight - k_turretHeight; - public static final double k_minYVelocityToHub = Math.sqrt(2.0 * k_gravitationalAcceleration * k_minYHeightToHub); - - } // TODO check and adjust constants + public static final double k_turretHeight = 2.0; // adjust to real design + public static final double k_extraTimeToPassSensor = 1.0; // test on field + } - - public static class FieldConstants { - public final static double k_width = Units.feetToMeters(26.0) + Units.inchesToMeters(5); - public final static double k_length = Units.feetToMeters(57.0) + Units.inchesToMeters(6.0 + (7.0 / 8.0)); + public static class FieldConstants { // feet (NOT INCHES) + public static final double k_hubWidth = 41.7 / 12.0; + public static final double k_hubRadius = hubWidth / Math.sqrt(3.0); + public static final double k_hubHeight = 6.0; + public static final double k_hubX = 0.0; // adjust to field coordinate convention + public static final double k_hubY = (158.6 + (47.0 / 2)) / 12.0; // same as above + public static final double k_fuelRadius = 5.91 / 12.0 / 2.0; + public static final double k_fuelMass = 0.474; // average + public static final double k_ceilingHeight = 15.0; // estimated } -} +} // TODO check and adjust constants From 8d2eb69d09c4cbada3b0c0ef399f0a2962ee22cd Mon Sep 17 00:00:00 2001 From: The-Candyman <147785024+The-Candyman@users.noreply.github.com> Date: Mon, 19 Jan 2026 05:57:34 +0000 Subject: [PATCH 06/12] Extra constant import oopsie --- src/main/java/frc/robot/subsystems/TurretSystem.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/subsystems/TurretSystem.java b/src/main/java/frc/robot/subsystems/TurretSystem.java index d78a0c3..d898e78 100644 --- a/src/main/java/frc/robot/subsystems/TurretSystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSystem.java @@ -9,6 +9,7 @@ import frc.robot.Constants.FieldConstants.k_hubX; import frc.robot.Constants.FieldConstants.k_hubY; import frc.robot.Constants.FieldConstants.k_fuelRadius; +import frc.robot.Constants.FieldConstants.k_ceilingHeight; public class TurretSystem extends SubsystemBase { From 67c7e101e558bb7902d746dafac8a0142664a200 Mon Sep 17 00:00:00 2001 From: Sarthak Ghoshal <139923826+websitedeb@users.noreply.github.com> Date: Fri, 23 Jan 2026 18:59:56 -0500 Subject: [PATCH 07/12] Revert "Big turret math drop" --- src/main/java/frc/robot/Constants.java | 8 +- .../frc/robot/subsystems/TurretSystem.java | 127 +++--------------- 2 files changed, 19 insertions(+), 116 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2992c61..d4e1c29 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,13 +25,7 @@ public static class ControllerConstants{ public static class TurretConstants { // feet (NOT INCHES), seconds, degrees, pounds (mass), pound*ft/s^2 (force) public static final double k_gravitationalAcceleration = 32.174; - public static final double k_turretHeight = 2.0; // adjust to real design - public static final double k_extraTimeToPassSensor = 1.0; // test on field - } - - public static class FieldConstants { // feet (NOT INCHES) - public static final double k_hubWidth = 41.7 / 12.0; - public static final double k_hubRadius = hubWidth / Math.sqrt(3.0); + public static final double k_turretHeight = 2.0; public static final double k_hubHeight = 6.0; public static final double k_hubX = 0.0; // adjust to field coordinate convention public static final double k_hubY = (158.6 + (47.0 / 2)) / 12.0; // same as above diff --git a/src/main/java/frc/robot/subsystems/TurretSystem.java b/src/main/java/frc/robot/subsystems/TurretSystem.java index d898e78..5e04073 100644 --- a/src/main/java/frc/robot/subsystems/TurretSystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSystem.java @@ -1,15 +1,8 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.TurretConstants.k_gravitationalAcceleration; -import frc.robot.Constants.TurretConstants.k_turretHeight; -import frc.robot.Constants.TurretConstants.k_extraTimeToPassSensor; -import frc.robot.Constants.FieldConstants.k_hubRadius; -import frc.robot.Constants.FieldConstants.k_hubHeight; -import frc.robot.Constants.FieldConstants.k_hubX; -import frc.robot.Constants.FieldConstants.k_hubY; -import frc.robot.Constants.FieldConstants.k_fuelRadius; -import frc.robot.Constants.FieldConstants.k_ceilingHeight; +import frc.robot.Constants; + public class TurretSystem extends SubsystemBase { @@ -18,103 +11,19 @@ public TurretSystem() { } - // TODO add desmos graph link - - // launch geometry and calculations - - public static double launchVelocity(double launchAngle, double xDistance, double yDistance) { - double divisor = 2.0 * cos(launchAngle) * (xDistance * sin(launchAngle) - yDistance * cos(launchAngle)); - double radicand = k_gravitationalAcceleration / divisor; - return xDistance * Math.sqrt(radicand); - } - public static double upperLaunchAngle(double launchVelocity, double xDistance, double yDistance) { - double distance = Math.hypot(xDistance, yDistance); - double targetFactor = yDistance / distance; - double dropFactor = k_gravitationalAcceleration * xDistance * xDistance / (launchVelocity * launchVelocity * distance); - return 0.5 * (arccos(targetFactor + dropFactor) + arccos(-targetFactor)); - } - public static double lowerLaunchAngle(double launchVelocity, double xDistance, double yDistance) { - double distance = Math.hypot(xDistance, yDistance); - double targetFactor = yDistance / distance; - double dropFactor = k_gravitationalAcceleration * xDistance * xDistance / (launchVelocity * launchVelocity * distance); - return 0.5 * (arcsin(targetFactor + dropFactor) + arcsin(targetFactor)); - } - public static double maxLaunchHeight(double launchAngle, double launchVelocity) { - double vYSquared = Math.pow(launchVelocity * sin(launchAngle), 2.0); - return 0.5 * vYSquared / k_gravitationalAcceleration; - } - public static double absHeightAtDistance(double launchAngle, double launchVelocity, double testX) { - double time = testX / (launchVelocity * cos(launchAngle)); - double launchFactor = time * launchVelocity * sin(launchAngle); - double dropFactor = 0.5 * time * k_gravitationalAcceleration * k_gravitationalAcceleration; - return k_turretHeight + launchFactor - dropFactor; - } - public static double secondTimeToAbsHeight(double launchAngle, double launchVelocity, double testHeight) { - double VxSin = launchVelocity * sin(launchAngle); - double radicand = VxSin * VxSin - 2 * k_gravitationalAcceleration * (testHeight - k_turretHeight); - return (VxSin + Math.sqrt(radicand)) / k_gravitationalAcceleration; - } - public static double farDistanceToAbsHeight(double launchAngle, double launchVelocity, double testHeight) { - return launchVelocity * cos(launchAngle) * secondTimeToAbsoluteHeight(launchAngle, launchVelocity, testHeight); - } - public static double trajectorySlopeAtDistance(double launchAngle, double launchVelocity, double testX) { - double xVelocity = launchVelocity * cos(launchAngle); - double time = testX / xVelocity; - double yVelocity = launchVelocity * sin(launchAngle) - time * k_gravitationalAcceleration; - return yVelocity / xVelocity; - } - public static boolean testValidHubTrajectory(double launchDirection, double launchAngle, double launchVelocity, - double turretX, double turretY, double sideClearance, double verticalClearance) { - // turret x, y, height should be measured as the location of the bottom of the fuel at the moment it is launched from the turret - // weather permitting, air resistance may be neglected - double distance = Math.hypot(k_hubX - turretX, k_hubY - turretY); - double sideOffset = Math.abs(cos(launchDirection) * (k_hubY - turretY) - sin(launchDirection) * (k_hubX - turretX)); - double launchDistance = Math.sqrt(distance * distance - sideOffset * sideOffset); - double nearEdgeDistance = launchDistance + hubNearEdgeOffset(launchDirection, sideOffset, 0.0); - double entranceDistance = farDistanceToAbsoluteHeight(launchAngle, launchVelocity, k_hubHeight); - double entranceHeight = absHeightAtDistance(launchAngle, launchVelocity, nearEdgeDistance); - double maxHeight = 2.0 * k_fuelRadius + maxLaunchHeight(launchAngle, launchVelocity); - double nearClearanceDistance = launchDistance - hubNearEdgeOffset(launchDirection, sideOffset, sideClearance); - double farClearanceingDistance = launchDistance + hubFarEdgeOffset(launchDirection, sideOffset, sideClearance); - boolean clearsHub = entranceHeight >= verticalClearance // measured at the center of the fuel entering the space above the funnel - && entranceDistance >= nearClearanceDistance // measured at the bottom of the fuel hitting 72 inches above the ground - && entranceDistance <= farClearanceDistance // clearance treats the funnel opening as if it has been shrunk - && maxLaunchHeight <= k_ceilingHeight; // it is admittedly unlikely that the turret will hit the ceiling, but still - return clearsHub; - } - public static double timeToHubScoring(double launchAngle, double launchVelocity) { - return k_extraTimeToPassSensor + secondTimeToAbsHeight(launchAngle, launchVelocity, k_hubHeight); - } - - // field geometry and calculations - - public static double hubNearEdgeOffset(double approachAngle, double sideOffset, double clearance) { - return hubFarEdgeOffset(approachAngle, -sideOffset, clearance); // Exploits the hub's hexagonal symmetry - } // Hexagons really are the bestagons... - public static double hubFarEdgeOffset(double approachAngle, double sideOffset, double clearance) { - double hubRadius = k_hubRadius - clearance * 2.0 / Math.sqrt(3.0); - double[] pXArr = new double[4]; - pXArr[0] = hubRadius * cos(210 - approachAngle % 60); - pXArr[1] = hubRadius * cos(150 - approachAngle % 60); - pXArr[2] = hubRadius * cos(90 - approachAngle % 60); - pXArr[3] = hubRadius * cos(30 - approachAngle % 60); - int intersectingSide = - (sideOffset < pXArr[0] || sideOffset > pXArr[3])? NAN : - ((sideOffset < pXArr[1])? 0 : - ((sideOffset < pXArr[2])? 1 : 2)); - double p1X = pXArr[intersectingSide]; - double p2X = pXArr[intersectingSide + 1]; - double p1Y = Math.sqrt(hubRadius * hubRadius - p1X * p1X); - double p2Y = Math.sqrt(hubRadius * hubRadius - p2X * p2X); - double p1Scale = (sideOffset - p2X) / (p1X - p2X); - double p2Scale = (sideOffset - p1X) / (p2X - p1X); - return p1Y * p1Scale + p2Y * p2Scale; - } - - // math helpers - - public static double sin(double degrees) {return Math.sin(Math.toRadians(degrees));} - public static double cos(double degrees) {return Math.cos(Math.toRadians(degrees));} - public static double arcsin(double ratio) {return Math.toDegrees(Math.asin(ratio));} - public static double arccos(double ratio) {return Math.toDegrees(Math.acos(ratio));} -} + public static double calculateLaunchVelocity(double launchAngle, double xDisplacement, double yDisplacement) { + double cos = Math.cos(Math.toRadians(launchAngle)); + double sin = Math.sin(Math.toRadians(launchAngle)); + double divisor = 2.0 * cos * (xDisplacement * sin - yDisplacement * cos); + double radicand = Constants.TurretConstants.k_gravitationalAcceleration / divisor; + return xDisplacement * Math.sqrt(radicand); + } + public static double calculateLaunchAngle(double launchVelocity, double xDisplacement, double yDisplacement) { + return 0.0; + } + public static double calculateMaxHeightFromLaunch(double launchAngle, double launchVelocity) { + double vYSquared = Math.pow(launchVelocity * Math.sin(Math.toRadians(launchAngle)), 2.0); + double twoG = 2.0 * Constants.TurretConstants.k_gravitationalAcceleration; + return vYSquared / twoG; + } +} \ No newline at end of file From ad0aad007cd37109866984de4177e391aa6ae28c Mon Sep 17 00:00:00 2001 From: "@ChaseMorsDeus101" Date: Sat, 24 Jan 2026 13:17:04 -0500 Subject: [PATCH 08/12] Intake File --- .../frc/robot/subsystems/IntakeSystem.java | 164 ++++++++++++++++++ 1 file changed, 164 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/IntakeSystem.java diff --git a/src/main/java/frc/robot/subsystems/IntakeSystem.java b/src/main/java/frc/robot/subsystems/IntakeSystem.java new file mode 100644 index 0000000..a8b7d25 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSystem.java @@ -0,0 +1,164 @@ +package frc.robot.subsystems; + +import com.thethriftybot.ThriftyNova; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.system.plant.DCMotor; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.DegreesPerSecond; +import static edu.wpi.first.units.Units.DegreesPerSecondPerSecond; +import static edu.wpi.first.units.Units.Feet; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Pounds; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Seconds; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import yams.gearing.GearBox; +import yams.gearing.MechanismGearing; +import yams.mechanisms.config.ArmConfig; +import yams.mechanisms.config.FlyWheelConfig; +import yams.mechanisms.positional.Arm; +import yams.mechanisms.velocity.FlyWheel; +import yams.motorcontrollers.SmartMotorController; +import yams.motorcontrollers.SmartMotorControllerConfig; +import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; +import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; +import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; +import yams.motorcontrollers.local.NovaWrapper; + +public class IntakeSubsystem extends SubsystemBase { + + private static final double INTAKE_SPEED = 1.0; + + // ThriftyNova controlling the intake roller + private ThriftyNova rollerNova = new ThriftyNova(Constants.IntakeConstants.kRollerMotorId); + + private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) + .withControlMode(ControlMode.OPEN_LOOP) + .withTelemetry("IntakeRollerMotor", TelemetryVerbosity.HIGH) + .withGearing(new MechanismGearing(GearBox.fromReductionStages(1))) // Direct drive, adjust if geared + .withMotorInverted(true) + .withIdleMode(MotorMode.COAST) + .withStatorCurrentLimit(Amps.of(40)); + + private SmartMotorController smc = new NovaWrapper(rollerNova, DCMotor.getNeoVortex(1), smcConfig); + + private final FlyWheelConfig intakeConfig = new FlyWheelConfig(smc) + .withDiameter(Inches.of(4)) + .withMass(Pounds.of(0.5)) + .withUpperSoftLimit(RPM.of(6000)) + .withLowerSoftLimit(RPM.of(-6000)) + .withTelemetry("IntakeRoller", TelemetryVerbosity.HIGH); + + private FlyWheel intake = new FlyWheel(intakeConfig); + + // 5:1, 5:1, 60/18 reduction + private SmartMotorControllerConfig intakePivotSmartMotorConfig = new SmartMotorControllerConfig(this) + .withControlMode(ControlMode.CLOSED_LOOP) + .withClosedLoopController(10, 0, 0, DegreesPerSecond.of(1080), DegreesPerSecondPerSecond.of(1080)) + .withFeedforward(new ArmFeedforward(0, 0, 1.3)) + .withTelemetry("IntakePivotMotor", TelemetryVerbosity.HIGH) + .withGearing(new MechanismGearing(GearBox.fromReductionStages(5, 5, 60.0 / 18.0, 42))) + .withMotorInverted(false) + .withIdleMode(MotorMode.COAST) + .withStatorCurrentLimit(Amps.of(10)) + .withClosedLoopRampRate(Seconds.of(0.1)); + + private ThriftyNova pivotMotor = new ThriftyNova(Constants.IntakeConstants.kPivotMotorId); + + private SmartMotorController intakePivotController = new NovaWrapper(pivotMotor, DCMotor.getNeoVortex(1), + intakePivotSmartMotorConfig); + + private final ArmConfig intakePivotConfig = new ArmConfig(intakePivotController) + .withSoftLimits(Degrees.of(0), Degrees.of(150)) + .withHardLimit(Degrees.of(0), Degrees.of(155)) + .withStartingPosition(Degrees.of(0)) + .withLength(Feet.of(1)) + .withMass(Pounds.of(2)) // Reis says: 2 pounds, not a lot + .withTelemetry("IntakePivot", TelemetryVerbosity.HIGH); + + private Arm intakePivot = new Arm(intakePivotConfig); + + public IntakeSubsystem() { + pivotMotor.factoryReset(); + } + + /** + * Command to run the intake while held. + */ + public Command intakeCommand() { + return intake.set(INTAKE_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Intake.Run"); + } + + /** + * Command to eject while held. + */ + public Command ejectCommand() { + return intake.set(-INTAKE_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Intake.Eject"); + } + + public Command setPivotAngle(Angle angle) { + return intakePivot.setAngle(angle).withName("IntakePivot.SetAngle"); + } + + public Command rezero() { + return Commands.runOnce(() -> pivotMotor.setEncoderPosition(0), this).withName("IntakePivot.Rezero"); + } + + /** + * Command to deploy intake and run roller while held. + * Stops roller when released. + */ + public Command deployAndRollCommand() { + return Commands.run(() -> { + setIntakeDeployed(); + smc.setDutyCycle(INTAKE_SPEED); + }, this).finallyDo(() -> { + smc.setDutyCycle(0); + setIntakeHold(); + }).withName("Intake.DeployAndRoll"); + } + + public Command backFeedAndRollCommand() { + return Commands.run(() -> { + setIntakeDeployed(); + // smc.setDutyCycle(-INTAKE_SPEED); + }, this).finallyDo(() -> { + smc.setDutyCycle(0); + setIntakeHold(); + }).withName("Intake.BackFeedAndRoll"); + } + + private void setIntakeStow() { + intakePivotController.setPosition(Degrees.of(0)); + } + + private void setIntakeFeed() { + intakePivotController.setPosition(Degrees.of(59)); + } + + private void setIntakeHold() { + intakePivotController.setPosition(Degrees.of(115)); + } + + private void setIntakeDeployed() { + intakePivotController.setPosition(Degrees.of(148)); + } + + @Override + public void periodic() { + intake.updateTelemetry(); + intakePivot.updateTelemetry(); + } + + @Override + public void simulationPeriodic() { + intake.simIterate(); + intakePivot.simIterate(); + } +} //Code graciously provided by CranberryAlarm \ No newline at end of file From fb1b70ac16f46e70764f99aa140b404f15f0042d Mon Sep 17 00:00:00 2001 From: Sarthak Ghoshal <139923826+websitedeb@users.noreply.github.com> Date: Sat, 31 Jan 2026 16:33:07 -0500 Subject: [PATCH 09/12] Revert "Intake File" --- .../frc/robot/subsystems/IntakeSystem.java | 164 ------------------ 1 file changed, 164 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/IntakeSystem.java diff --git a/src/main/java/frc/robot/subsystems/IntakeSystem.java b/src/main/java/frc/robot/subsystems/IntakeSystem.java deleted file mode 100644 index a8b7d25..0000000 --- a/src/main/java/frc/robot/subsystems/IntakeSystem.java +++ /dev/null @@ -1,164 +0,0 @@ -package frc.robot.subsystems; - -import com.thethriftybot.ThriftyNova; - -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.system.plant.DCMotor; -import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.DegreesPerSecond; -import static edu.wpi.first.units.Units.DegreesPerSecondPerSecond; -import static edu.wpi.first.units.Units.Feet; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Pounds; -import static edu.wpi.first.units.Units.RPM; -import static edu.wpi.first.units.Units.Seconds; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import yams.gearing.GearBox; -import yams.gearing.MechanismGearing; -import yams.mechanisms.config.ArmConfig; -import yams.mechanisms.config.FlyWheelConfig; -import yams.mechanisms.positional.Arm; -import yams.mechanisms.velocity.FlyWheel; -import yams.motorcontrollers.SmartMotorController; -import yams.motorcontrollers.SmartMotorControllerConfig; -import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; -import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; -import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; -import yams.motorcontrollers.local.NovaWrapper; - -public class IntakeSubsystem extends SubsystemBase { - - private static final double INTAKE_SPEED = 1.0; - - // ThriftyNova controlling the intake roller - private ThriftyNova rollerNova = new ThriftyNova(Constants.IntakeConstants.kRollerMotorId); - - private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) - .withControlMode(ControlMode.OPEN_LOOP) - .withTelemetry("IntakeRollerMotor", TelemetryVerbosity.HIGH) - .withGearing(new MechanismGearing(GearBox.fromReductionStages(1))) // Direct drive, adjust if geared - .withMotorInverted(true) - .withIdleMode(MotorMode.COAST) - .withStatorCurrentLimit(Amps.of(40)); - - private SmartMotorController smc = new NovaWrapper(rollerNova, DCMotor.getNeoVortex(1), smcConfig); - - private final FlyWheelConfig intakeConfig = new FlyWheelConfig(smc) - .withDiameter(Inches.of(4)) - .withMass(Pounds.of(0.5)) - .withUpperSoftLimit(RPM.of(6000)) - .withLowerSoftLimit(RPM.of(-6000)) - .withTelemetry("IntakeRoller", TelemetryVerbosity.HIGH); - - private FlyWheel intake = new FlyWheel(intakeConfig); - - // 5:1, 5:1, 60/18 reduction - private SmartMotorControllerConfig intakePivotSmartMotorConfig = new SmartMotorControllerConfig(this) - .withControlMode(ControlMode.CLOSED_LOOP) - .withClosedLoopController(10, 0, 0, DegreesPerSecond.of(1080), DegreesPerSecondPerSecond.of(1080)) - .withFeedforward(new ArmFeedforward(0, 0, 1.3)) - .withTelemetry("IntakePivotMotor", TelemetryVerbosity.HIGH) - .withGearing(new MechanismGearing(GearBox.fromReductionStages(5, 5, 60.0 / 18.0, 42))) - .withMotorInverted(false) - .withIdleMode(MotorMode.COAST) - .withStatorCurrentLimit(Amps.of(10)) - .withClosedLoopRampRate(Seconds.of(0.1)); - - private ThriftyNova pivotMotor = new ThriftyNova(Constants.IntakeConstants.kPivotMotorId); - - private SmartMotorController intakePivotController = new NovaWrapper(pivotMotor, DCMotor.getNeoVortex(1), - intakePivotSmartMotorConfig); - - private final ArmConfig intakePivotConfig = new ArmConfig(intakePivotController) - .withSoftLimits(Degrees.of(0), Degrees.of(150)) - .withHardLimit(Degrees.of(0), Degrees.of(155)) - .withStartingPosition(Degrees.of(0)) - .withLength(Feet.of(1)) - .withMass(Pounds.of(2)) // Reis says: 2 pounds, not a lot - .withTelemetry("IntakePivot", TelemetryVerbosity.HIGH); - - private Arm intakePivot = new Arm(intakePivotConfig); - - public IntakeSubsystem() { - pivotMotor.factoryReset(); - } - - /** - * Command to run the intake while held. - */ - public Command intakeCommand() { - return intake.set(INTAKE_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Intake.Run"); - } - - /** - * Command to eject while held. - */ - public Command ejectCommand() { - return intake.set(-INTAKE_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Intake.Eject"); - } - - public Command setPivotAngle(Angle angle) { - return intakePivot.setAngle(angle).withName("IntakePivot.SetAngle"); - } - - public Command rezero() { - return Commands.runOnce(() -> pivotMotor.setEncoderPosition(0), this).withName("IntakePivot.Rezero"); - } - - /** - * Command to deploy intake and run roller while held. - * Stops roller when released. - */ - public Command deployAndRollCommand() { - return Commands.run(() -> { - setIntakeDeployed(); - smc.setDutyCycle(INTAKE_SPEED); - }, this).finallyDo(() -> { - smc.setDutyCycle(0); - setIntakeHold(); - }).withName("Intake.DeployAndRoll"); - } - - public Command backFeedAndRollCommand() { - return Commands.run(() -> { - setIntakeDeployed(); - // smc.setDutyCycle(-INTAKE_SPEED); - }, this).finallyDo(() -> { - smc.setDutyCycle(0); - setIntakeHold(); - }).withName("Intake.BackFeedAndRoll"); - } - - private void setIntakeStow() { - intakePivotController.setPosition(Degrees.of(0)); - } - - private void setIntakeFeed() { - intakePivotController.setPosition(Degrees.of(59)); - } - - private void setIntakeHold() { - intakePivotController.setPosition(Degrees.of(115)); - } - - private void setIntakeDeployed() { - intakePivotController.setPosition(Degrees.of(148)); - } - - @Override - public void periodic() { - intake.updateTelemetry(); - intakePivot.updateTelemetry(); - } - - @Override - public void simulationPeriodic() { - intake.simIterate(); - intakePivot.simIterate(); - } -} //Code graciously provided by CranberryAlarm \ No newline at end of file From 3566163fb19c62fbdc17282ec45402cc4a47f4fa Mon Sep 17 00:00:00 2001 From: paragrghoshal Date: Tue, 3 Feb 2026 19:03:46 -0500 Subject: [PATCH 10/12] added Studica lib + swerveModules.json + controllerprops.json --- .../deploy/swerve/controllerproperties.json | 8 +++ src/main/deploy/swerve/swerveModules.json | 14 ++++ vendordeps/Studica.json | 71 +++++++++++++++++++ 3 files changed, 93 insertions(+) create mode 100644 src/main/deploy/swerve/controllerproperties.json create mode 100644 src/main/deploy/swerve/swerveModules.json create mode 100644 vendordeps/Studica.json diff --git a/src/main/deploy/swerve/controllerproperties.json b/src/main/deploy/swerve/controllerproperties.json new file mode 100644 index 0000000..669097e --- /dev/null +++ b/src/main/deploy/swerve/controllerproperties.json @@ -0,0 +1,8 @@ +{ + "angleJoystickRadiusDeadband": 0.5, + "heading": { + "p": 0.4, + "i": 0, + "d": 0.01 + } +} \ No newline at end of file diff --git a/src/main/deploy/swerve/swerveModules.json b/src/main/deploy/swerve/swerveModules.json new file mode 100644 index 0000000..eaa4400 --- /dev/null +++ b/src/main/deploy/swerve/swerveModules.json @@ -0,0 +1,14 @@ +{ + "imu": { + "type": "navx", + "id": 0, + "canbus": null + }, + "invertedIMU": false, + "modules": [ + "frontleft.json", + "frontright.json", + "backleft.json", + "backright.json" + ] +} \ No newline at end of file diff --git a/vendordeps/Studica.json b/vendordeps/Studica.json new file mode 100644 index 0000000..b51bf58 --- /dev/null +++ b/vendordeps/Studica.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica.json", + "name": "Studica", + "version": "2026.0.0", + "frcYear": "2026", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2026/" + ], + "jsonUrl": "https://dev.studica.com/maven/release/2026/json/Studica-2026.0.0.json", + "javaDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "Studica-java", + "version": "2026.0.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "Studica-driver", + "version": "2026.0.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "Studica-cpp", + "version": "2026.0.0", + "libName": "Studica", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.studica.frc", + "artifactId": "Studica-driver", + "version": "2026.0.0", + "libName": "StudicaDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From 99ac84adfe0e96affb8db1a752ef58f18602c5fe Mon Sep 17 00:00:00 2001 From: paragrghoshal Date: Tue, 3 Feb 2026 20:33:30 -0500 Subject: [PATCH 11/12] fixed naming --- src/main/deploy/swerve/{swerveModules.json => swerveDrive.json} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/main/deploy/swerve/{swerveModules.json => swerveDrive.json} (100%) diff --git a/src/main/deploy/swerve/swerveModules.json b/src/main/deploy/swerve/swerveDrive.json similarity index 100% rename from src/main/deploy/swerve/swerveModules.json rename to src/main/deploy/swerve/swerveDrive.json From 31c5d2e3d97fc8ce7e02090ba884961dec3e6f21 Mon Sep 17 00:00:00 2001 From: Gary Yu <122239725+Toxgen@users.noreply.github.com> Date: Tue, 24 Feb 2026 20:01:38 -0500 Subject: [PATCH 12/12] Refactor update rejection logic in LimelightSystem --- .../frc/robot/subsystems/LimelightSystem.java | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/LimelightSystem.java b/src/main/java/frc/robot/subsystems/LimelightSystem.java index 8dd2d23..c3d0b00 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSystem.java +++ b/src/main/java/frc/robot/subsystems/LimelightSystem.java @@ -54,8 +54,8 @@ public LimelightSystem(SwerveDrive swerve){ public void periodic() { // if the pose is there visionEstimate.ifPresent((PoseEstimate poseEstimate) -> { - this.allowed = this.rejectUpdate(poseEstimate); - if (!this.allowed) { + this.allowed = this.doRejectUpdate(poseEstimate); + if (this.allowed) { swerveDrive.addVisionMeasurement( poseEstimate.pose.toPose2d(), poseEstimate.timestampSeconds); @@ -72,18 +72,18 @@ public void onEnabled() { /* */ - public boolean rejectUpdate(PoseEstimate poseEstimate) { + public boolean doRejectUpdate(PoseEstimate poseEstimate) { /* returns true if Pose didn't pass tests returns false if Pose passed tests */ - if (poseEstimate.getAvgTagAmbiguity() > 0.7 ) { return true; } - if (poseEstimate.pose.getX() <= 0 || poseEstimate.pose.getX() > Constants.FieldConstants.k_length) { return true; } - if (poseEstimate.pose.getY() <= 0 || poseEstimate.pose.getY() > Constants.FieldConstants.k_width) { return true; } - if (Math.abs(swerveDrive.getRobotVelocity().omegaRadiansPerSecond) > Math.PI * 2) { return true; } - if (Math.abs(swerveDrive.getRobotVelocity().omegaRadiansPerSecond) > Math.PI * 2) { return true; } + if (poseEstimate.getAvgTagAmbiguity() > 0.7 ) { return false; } + if (poseEstimate.pose.getX() <= 0 || poseEstimate.pose.getX() > Constants.FieldConstants.k_length) { return false; } + if (poseEstimate.pose.getY() <= 0 || poseEstimate.pose.getY() > Constants.FieldConstants.k_width) { return false; } + if (Math.abs(swerveDrive.getRobotVelocity().omegaRadiansPerSecond) > Math.PI * 2) { return false; } + if (Math.abs(swerveDrive.getRobotVelocity().omegaRadiansPerSecond) > Math.PI * 2) { return false; } - return false; + return true; } -} \ No newline at end of file +}