Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025

simgui-ds.json

@@ -1,4 +1,9 @@
 {
+  "Joysticks": {
+    "window": {
+      "visible": false
+    }
+  },
   "System Joysticks": {
     "window": {
       "enabled": false

src/main/deploy/pathplanner/paths/BlueBasChercher.path

@@ -11,7 +11,7 @@
         "x": 4.699180327868851,
         "y": 2.1728995901639334
       },
-      "isLocked": false,
+      "isLocked": true,
       "linkedName": null
     },
     {
@@ -27,7 +27,7 @@
         "x": 2.853313965378558,
         "y": 1.5363479888253109
       },
-      "isLocked": false,
+      "isLocked": true,
       "linkedName": null
     },
     {
@@ -40,7 +40,7 @@
         "y": 1.3902362024139756
       },
       "nextControl": null,
-      "isLocked": false,
+      "isLocked": true,
       "linkedName": null
     }
   ],

src/main/deploy/pathplanner/paths/New Path.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.564241803278687,
+        "x": 16.243,
-        "y": 3.899129098360655
+        "y": 6.55025
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.506996649367605,
+        "x": 15.354412756981612,
-        "y": 3.898163120386143
+        "y": 6.548572497113415
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.269569672131147,
+        "x": 1.3552499999999998,
-        "y": 3.899129098360655
+        "y": 6.55025
       },
       "prevControl": {
-        "x": 7.259410179761819,
+        "x": 2.306674365666888,
-        "y": 3.9002985318003183
+        "y": 6.526815183428469
       },
       "nextControl": null,
       "isLocked": false,
@@ -34,7 +34,7 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 0.1,
     "maxAngularAcceleration": 0.1,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/settings.json

@@ -9,7 +9,7 @@
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
-  "robotMass": 45.3592,
+  "robotMass": 45.359,
   "robotMOI": 6.883,
   "robotTrackwidth": 0.546,
   "driveWheelRadius": 0.048,

src/main/java/frc/robot/RobotContainer.java

@@ -10,21 +10,21 @@ import java.util.Map;
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.hardware.Pigeon2;
 import com.ctre.phoenix6.swerve.SwerveRequest;
+import com.pathplanner.lib.auto.AutoBuilder;
+
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
+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.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.TunerConstants.TunerConstants;
-import frc.robot.commands.RainBow;
 import frc.robot.commands.reset;
 import frc.robot.commands.Elevateur.Depart;
 import frc.robot.commands.Elevateur.ElevateurManuel;
@@ -59,6 +59,7 @@ import frc.robot.commands.Elevateur.balonL2;
 import frc.robot.commands.Elevateur.balonL3;
 
 public class RobotContainer {
+  private final SendableChooser<Command> autoChooser;
   ShuffleboardLayout layoutauto = Shuffleboard.getTab("teb").getLayout("auto", BuiltInLayouts.kList)
     .withSize(2, 2).withProperties(Map.of("Label position", "LEFT"));
   GenericEntry L1 = layoutauto.add("choix L1",false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry();
@@ -91,16 +92,16 @@ public class RobotContainer {
   Bougie bougie = new Bougie();
   Limelight3G limelight3g = new Limelight3G();
   Limelight3 limelight3 = new Limelight3();
-  Pose2d pose = new Pose2d();
   Requin requin = new Requin();
   CorailAspir corailAspir = new CorailAspir(pince, bougie);
   public RobotContainer() {
+  autoChooser = AutoBuilder.buildAutoChooser();
   CameraServer.startAutomaticCapture();
+   SmartDashboard.putData("Auto Chooser", autoChooser);
   configureBindings();
   }
 
   private void configureBindings() {
-    drivetrain.registerTelemetry(logger::telemeterize);
     drivetrain.setDefaultCommand(
       // Drivetrain will execute this command periodically
           drivetrain.applyRequest(() ->
@@ -115,7 +116,7 @@ public class RobotContainer {
     manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie));
     manette1.rightBumper().toggleOnTrue(new StationPince(pince, elevateur,bougie));
     manette1.povLeft().whileTrue(new AlgueExpire(pince, bougie));
-    manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY));
+    // manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain1,manette1::getLeftX,manette1::getLeftY));
     manette1.povRight().whileTrue(new CoralExpire(pince, bougie));
     manette1.leftTrigger().whileTrue(new DepartPince(pince));
     manette1.povDown().whileTrue(new Algue_inspire(pince,bougie));
@@ -139,8 +140,8 @@ public class RobotContainer {
     manette2.x().whileTrue(new ExpireCorail(requin, bougie));
 
     //limelight
-    manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
+    // manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
-    manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY));
+    // manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY));
     
     //Pince manuel
     pince.setDefaultCommand(new RunCommand(()->{
@@ -151,40 +152,42 @@ public class RobotContainer {
     elevateur.setDefaultCommand(new RunCommand(()->{
     elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY()*manette2.getLeftY()*manette2.getLeftY(), 0.05));
     }, elevateur));
-
+    
     //Reset encodeur
     manette2.start().whileTrue(new reset(elevateur, pince, requin));
+    drivetrain.registerTelemetry(logger::telemeterize);
   }
     
     public Command getAutonomousCommand() {
-      return new SequentialCommandGroup(
+      return autoChooser.getSelected();
-        drivetrain.applyRequest(()->
+  //     return new SequentialCommandGroup(
-        drive.withVelocityX(0.1*MaxSpeed)
+  //       drivetrain.applyRequest(()->
-       .withVelocityY(0)
+  //       drive.withVelocityX(0.1*MaxSpeed)
-       .withRotationalRate(0)).unless(()->!ReculerR.getBoolean(true)).withTimeout(3.5),
+  //      .withVelocityY(0)
-       drivetrain.applyRequest(()->
+  //      .withRotationalRate(0)).unless(()->!ReculerR.getBoolean(true)).withTimeout(3.5),
-       drive.withVelocityX(-0.1*MaxSpeed)
+  //      drivetrain.applyRequest(()->
-      .withVelocityY(0)
+  //      drive.withVelocityX(-0.1*MaxSpeed)
-      .withRotationalRate(0)).unless(()->!ReculerB.getBoolean(true)).withTimeout(3.5),
+  //     .withVelocityY(0)
-        drivetrain.applyRequest(()->
+  //     .withRotationalRate(0)).unless(()->!ReculerB.getBoolean(true)).withTimeout(3.5),
-         drive.withVelocityX(-0.1*MaxSpeed)
+  //       drivetrain.applyRequest(()->
-        .withVelocityY(0)
+  //        drive.withVelocityX(-0.1*MaxSpeed)
-        .withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(2.35),
+  //       .withVelocityY(0)
-     drivetrain.applyRequest(()->
+  //       .withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(2.35),
-     drive.withVelocityX(0.1*MaxSpeed)
+  //    drivetrain.applyRequest(()->
-    .withVelocityY(0)
+  //    drive.withVelocityX(0.1*MaxSpeed)
-    .withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerR.getBoolean(true)).withTimeout(3),
+  //   .withVelocityY(0)
-    drivetrain.applyRequest(()->
+  //   .withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerR.getBoolean(true)).withTimeout(3),
-    drive.withVelocityX(-0.1*MaxSpeed)
+  //   drivetrain.applyRequest(()->
-   .withVelocityY(0)
+  //   drive.withVelocityX(-0.1*MaxSpeed)
-   .withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerB.getBoolean(true)).withTimeout(3),
+  //  .withVelocityY(0)
-       drivetrain.applyRequest(()->
+  //  .withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerB.getBoolean(true)).withTimeout(3),
-        drive.withVelocityX(0)
+  //      drivetrain.applyRequest(()->
-       .withVelocityY(0)
+  //       drive.withVelocityX(0)
-       .withRotationalRate(0)).withTimeout(0.1),
+  //      .withVelocityY(0)
-        new L4(elevateur, pince).unless(()->!L4.getBoolean(true)).withTimeout(4),
+  //      .withRotationalRate(0)).withTimeout(0.1),
-        new L1Requin(requin, bougie).unless(()-> !L1.getBoolean(true)).withTimeout(2),
+  //       new L4(elevateur, pince).unless(()->!L4.getBoolean(true)).withTimeout(4),
-        new ExpireCorail(requin, bougie).unless(()->!L1.getBoolean(true)).withTimeout(2),
+  //       new L1Requin(requin, bougie).unless(()-> !L1.getBoolean(true)).withTimeout(2),
-        new RainBow(bougie));
+  //       new ExpireCorail(requin, bougie).unless(()->!L1.getBoolean(true)).withTimeout(2),
-    }    
+  //       new RainBow(bougie));
+     }    
 }

src/main/java/frc/robot/commands/Limelight/AprilTag3.java

@@ -5,11 +5,16 @@
 package frc.robot.commands.Limelight;
 import static edu.wpi.first.units.Units.*;
 
+import java.util.Optional;
 import java.util.function.DoubleSupplier;
 
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
-
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.SynchronousInterrupt.WaitResult;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.TunerConstants.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
@@ -47,16 +52,34 @@ public class AprilTag3 extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double a = limelight3.getTx();
+    double[] BotPose = new double[6];
-  double b = (limelight3.getTA()*100) / (Math.cos(90-limelight3.getTx()));
+    Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
-    if(limelight3.getV() == true){
+    if(alliance.get() == DriverStation.Alliance.Red){
-      drivetrain.setControl(drive.
+       BotPose = limelight3.getBotPoseRed();
-      withRotationalRate(a/20).
-      withVelocityX(0).
-      withVelocityY( b/50));
-       System.out.println(b/50);
     }
     else{
+      BotPose = limelight3.getBotPoseBlue();
+    }
+    double botx = BotPose[0];
+    double boty = BotPose[1];
+    double tx = limelight3.getTx();
+    double tagId = limelight3.getTId();
+    if(limelight3.getV() == true){
+      if(tagId ==10){
+        drivetrain.setControl(drive.
+        withRotationalRate(tx/20).
+        withVelocityX((botx-5.81)*2).
+        withVelocityY((boty-4)*4));
+      }
+      else if(tagId ==7){
+        drivetrain.setControl(drive.
+        withRotationalRate(tx/20).
+        withVelocityX(2-botx).
+        withVelocityY(2-boty));
+
+      }
+  }
+  else{
       drivetrain.setControl(drive.
       withRotationalRate(0).
       withVelocityX(0).

src/main/java/frc/robot/commands/Pince/Algue_inspire.java

@@ -31,7 +31,7 @@ public class Algue_inspire extends Command {
   @Override
   public void execute() {
     
-    if(pince.emperagealgue()>60){
+    if(pince.emperagealgue()>90){
       pince.aspirealgue(0);
       bougie.Bleu();
     }

src/main/java/frc/robot/commands/Pince/CoralExpire.java

@@ -32,7 +32,7 @@ public class CoralExpire extends Command {
    }
     */
     pince.aspirecoral(-.5);
-    bougie.Jaune();
+    bougie.Rouge();
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/subsystems/Bougie.java

@@ -22,20 +22,46 @@ public class Bougie extends SubsystemBase {
   CANdleConfiguration config = new CANdleConfiguration();
   LarsonAnimation rainbowAnim = new LarsonAnimation(256,0,0,0,0.1,68,BounceMode.Front,10,8);
   //TwinkleOffAnimation rainbowAnim = new TwinkleOffAnimation(256, 0, 0,0,0.5,68,TwinkleOffPercent.Percent88,8);
+  int led = 16;
+  int led2 = 8;
+  boolean x =true;
   /** Creates a new Bougie. */
   public Bougie() {
     config.brightnessScalar = 0.5;
     candle.configAllSettings(config);
   }
   public void Rouge() {
-   candle.setLEDs(255, 0, 0,0,8,8);
+    if(x){
-   candle.setLEDs(255, 0, 0,0,24,8);
+      candle.setLEDs(255, 0, 0,0,led,8);
-   candle.setLEDs(255, 0, 0,0,40,8);
+      led++;
-   candle.setLEDs(255, 0, 0,0,56,8);
+      candle.setLEDs(0, 0, 0,0,led2,8); 
-   candle.setLEDs(255, 0, 0,0,72,8);
+      led2++; 
-   candle.setLEDs(255, 0, 0,0,88,8);
+      System.out.println("led monte");
-   candle.setLEDs(255, 0, 0,0,104,8);
+      System.out.println(led); 
-   candle.setLEDs(255, 0, 0,0,120,8);
+       if(led>=68){
+        x=false;
+        System.out.println("true"); 
+        System.out.println(x); 
+      } 
+     } 
+     if(!x){
+      candle.setLEDs(255, 0, 0,0,led2,8);
+      led2--;
+      candle.setLEDs(0, 0, 0,0,led,8); 
+      led--;
+      System.out.println("leds descendent"); 
+      
+      if(led==15){
+        x=true;
+        System.out.println("false"); 
+  }}
+  //  candle.setLEDs(255, 0, 0,0,24,8);
+  //  candle.setLEDs(255, 0, 0,0,40,8);
+  //  candle.setLEDs(255, 0, 0,0,56,8);
+  //  candle.setLEDs(255, 0, 0,0,72,8);
+  //  candle.setLEDs(255, 0, 0,0,88,8);
+  //  candle.setLEDs(255, 0, 0,0,104,8);
+  //  candle.setLEDs(255, 0, 0,0,120,8);
   }
   public void Vert() {
    candle.setLEDs(0, 255, 0,0,8,8);

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -1,5 +1,6 @@
 package frc.robot.subsystems;
 
+
 import java.util.function.Supplier;
 import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
@@ -11,6 +12,8 @@ import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -30,26 +33,49 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     private static final double kSimLoopPeriod = 0.005; // 5 ms
     private Notifier m_simNotifier = null;
     private double m_lastSimTime;
-
     /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
     private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
     /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
     private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
     /* Keep track if we've ever applied the operator perspective before or not */
     private boolean m_hasAppliedOperatorPerspective = false;
-
     private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds();
-
+// Position des modules par rapport au centre du robot
+Translation2d frontLeftLocation  = new Translation2d(+0.3, +0.3);
+Translation2d frontRightLocation = new Translation2d(+0.3, -0.3);
+Translation2d backLeftLocation   = new Translation2d(-0.3, +0.3);
+Translation2d backRightLocation  = new Translation2d(-0.3, -0.3);
     /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
-    private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation();
+    private void configureAutoBuilder() {
-    private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
+        try {
-    private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
+            RobotConfig config = RobotConfig.fromGUISettings();
-
+            AutoBuilder.configure(
-    // private void configureAutoBuilder() {
+                () -> getState().Pose,   // Supplier of current robot pose
+                this::resetPose,         // Consumer for seeding pose against auto
+                () -> getState().Speeds, // Supplier of current robot speeds
+                // Consumer of ChassisSpeeds and feedforwards to drive the robot
+                (speeds, feedforwards) -> setControl(
+                    m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))
+                        .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
+                        .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
+                ),
+                new PPHolonomicDriveController(
+                    // PID constants for translation
+                    new PIDConstants(10, 0, 0),
+                    // PID constants for rotation
+                    new PIDConstants(7, 0, 0)
+                ),
+                config,
+                // Assume the path needs to be flipped for Red vs Blue, this is normally the case
+                () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
+                this // Subsystem for requirements
+            );
+        } catch (Exception ex) {
+            DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
+        }
     //     try {
     //         var config = RobotConfig.fromGUISettings();
-    //         AutoBuilder.configure(
+    //         AutoBuilder.configure(  
-                
     //             () -> getState().Pose,   // Supplier of current robot pose
     //             this::resetPose,         // Consumer for seeding pose against auto
     //             () -> getState().Speeds, // Supplier of current robot speeds
@@ -62,11 +88,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     //             ),
     //             new PPHolonomicDriveController(
     //                 // PID constants for translation
-    //                 new PIDConstants(63.167, 0, 0.54521),
+    //                 new PIDConstants(5, 0, 0),
     //                 // // PID constants for rotation
     //                 // new PIDConstants(7.9735, 0, 0.038499)
     //                 // PID constants for rotation
-    //                 new PIDConstants(43.502,0,2.7353)
+    //                 new PIDConstants(5,0,0)
     //             ),
     //             config,
     //             // Assume the path needs to be flipped for Red vs Blue, this is normally the case
@@ -77,11 +103,24 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     //     } catch (Exception ex) {
     //         DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
     //     }
-    //      PPHolonomicDriveController.overrideRotationFeedback(()->{
+    //       PPHolonomicDriveController.overrideRotationFeedback(()->{
-    //         return 0;
+    //          return 0;
-    //      });  
+    //       });  
-    //     }
+         }
+//         public void driveRobotRelative(ChassisSpeeds speeds) {
 
+//     // Convertir robot-relative ChassisSpeeds → SwerveModuleStates
+//     SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);
+
+//     // Limitations des roues
+//     SwerveDriveKinematics.desaturateWheelSpeeds(states, TunerConstants.kSpeedAt12Volts);
+
+//     // Envoi aux modules
+//     frontLeft.setDesiredState(states[0]);
+//     frontRight.setDesiredState(states[1]);
+//     backLeft.setDesiredState(states[2]);
+//     backRight.setDesiredState(states[3]);
+// }
     /**
      * Constructs a CTRE SwerveDrivetrain using the specified constants.
      * <p>
@@ -100,9 +139,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         if (Utils.isSimulation()) {
             startSimThread();
         }
-        //configureAutoBuilder();
+    configureAutoBuilder();
     }
-
     /**
      * Constructs a CTRE SwerveDrivetrain using the specified constants.
      * <p>
@@ -125,7 +163,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         if (Utils.isSimulation()) {
             startSimThread();
         }
-     //   configureAutoBuilder();
+        configureAutoBuilder();
     }
 
     /**
@@ -158,7 +196,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         if (Utils.isSimulation()) {
             startSimThread();
         }
-        //configureAutoBuilder();
+        configureAutoBuilder();
     }
 
     

src/main/java/frc/robot/subsystems/Limelight3.java

@@ -8,6 +8,7 @@ import edu.wpi.first.net.PortForwarder;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.LimelightHelpers;
 
@@ -16,10 +17,29 @@ public class Limelight3 extends SubsystemBase {
   NetworkTableEntry pipeline = table.getEntry("pipeline");
   /** Creates a new Limelight3. */
   public Limelight3() {
+    
     for(int port = 5800; port <=5807; port++){
       PortForwarder.add(port, "limelight.local", port);
     }
   }
+  public double[] getTargetPose(){
+    NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
+  NetworkTableEntry targetPoseEntry = limelightTable.getEntry("targetpose_cameraspace");
+  double[] targetPose = targetPoseEntry.getDoubleArray(new double[6]);
+  return targetPose;
+  }
+  public double[] getBotPoseBlue(){
+  NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
+  NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue");
+  double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
+  return BotPose;
+  }
+  public double[] getBotPoseRed(){
+    NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");
+  NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired");
+  double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
+  return BotPose;
+  }
   public double getTx(){
     NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
     NetworkTableEntry tx = table.getEntry("tx");
@@ -30,6 +50,11 @@ public class Limelight3 extends SubsystemBase {
     NetworkTableEntry ta = table.getEntry("ta");
     return ta.getDouble(0.0);
   }
+  public double getTId(){
+    NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
+    NetworkTableEntry tid = table.getEntry("tid");
+    return tid.getDouble(0.0);
+  }
   public double getRx(){
     NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
     NetworkTableEntry rx = table.getEntry("rx");

vendordeps/PathplannerLib-2025.2.6.json

@@ -1,7 +1,7 @@
 {
-    "fileName": "PathplannerLib-2025.2.3.json",
+    "fileName": "PathplannerLib-2025.2.6.json",
     "name": "PathplannerLib",
-    "version": "2025.2.3",
+    "version": "2025.2.6",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2025",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2025.2.3"
+            "version": "2025.2.6"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2025.2.3",
+            "version": "2025.2.6",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,

vendordeps/Phoenix6-frc2025-latest.json

@@ -1,7 +1,7 @@
 {
-    "fileName": "Phoenix6-25.2.2.json",
+    "fileName": "Phoenix6-frc2025-latest.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "25.2.2",
+    "version": "25.4.0",
     "frcYear": "2025",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "25.2.2"
+            "version": "25.4.0"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,7 +152,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -194,7 +194,35 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.2.2",
+            "version": "25.4.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdi",
+            "version": "25.4.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdle",
+            "version": "25.4.0",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -210,7 +238,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -226,7 +254,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -242,7 +270,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -258,7 +286,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -274,7 +302,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -290,7 +318,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -306,7 +334,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -322,7 +350,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -338,7 +366,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -354,7 +382,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "libName": "CTRE_SimProTalonFXS",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -370,7 +398,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -386,7 +414,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -402,7 +430,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.2.2",
+            "version": "25.4.0",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -414,6 +442,38 @@
                 "osxuniversal"
             ],
             "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdi",
+            "version": "25.4.0",
+            "libName": "CTRE_SimProCANdi",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANdle",
+            "version": "25.4.0",
+            "libName": "CTRE_SimProCANdle",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
         }
     ]
 }