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/.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/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/swerveDrive.json b/src/main/deploy/swerve/swerveDrive.json new file mode 100644 index 0000000..eaa4400 --- /dev/null +++ b/src/main/deploy/swerve/swerveDrive.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/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4130dbe..d4e1c29 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); } @@ -22,17 +27,10 @@ public static class TurretConstants { // feet (NOT INCHES), seconds, degrees, po 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 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 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 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cea1ffc..6582897 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..c3d0b00 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,24 +36,25 @@ 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(){ - //if the pose is there + public void periodic() { + // if the pose is there visionEstimate.ifPresent((PoseEstimate poseEstimate) -> { - this.allowed = this.exceptions(poseEstimate); + this.allowed = this.doRejectUpdate(poseEstimate); if (this.allowed) { swerveDrive.addVisionMeasurement( poseEstimate.pose.toPose2d(), @@ -61,23 +63,27 @@ 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; } - - // TODO make sure the april tag area is legibi + /* + + */ + 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 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 true; - } -} \ 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 2b88066..5b5c011 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 { @@ -38,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); @@ -74,7 +75,7 @@ public SwerveSystem() { @Override public void periodic(){ - m_limelight.periodic(); + DriverController.testGetControllerButtonA(); } @Override 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 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