merge limelight to main

.roboRIOTeamNumberSetter/roborioteamnumbersetter.json

@@ -0,0 +1,3 @@
+{
+  "TeamNumber": 1741
+}

.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
     }
   ]
 }

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 {

src/main/deploy/swerve/controllerproperties.json

@@ -0,0 +1,8 @@
+{
+  "angleJoystickRadiusDeadband": 0.5,
+  "heading": {
+    "p": 0.4,
+    "i": 0,
+    "d": 0.01
+  }
+}

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"
+  ]
+}

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

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

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();

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?

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;
-
     }
-}
+}

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

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": []
+}