From 77ce877923da4910b9ca81a2b9ab239be30565c5 Mon Sep 17 00:00:00 2001
From: Samuel <2540638@etudiant.cegepvicto.com>
Date: Mon, 17 Nov 2025 18:01:18 -0500
Subject: [PATCH 1/2] limelightTest

---
 .../robot/commands/Limelight/AprilTag3.java   | 47 ++++++++-----------
 .../java/frc/robot/subsystems/Limelight3.java | 12 +++--
 2 files changed, 28 insertions(+), 31 deletions(-)

diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java
index 78ad1da..54bb595 100644
--- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java
+++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java
@@ -5,12 +5,15 @@
 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.MathUtil;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.TunerConstants.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
@@ -48,38 +51,26 @@ public class AprilTag3 extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double[] targetPose = limelight3.getTargetPose();
-    double[] BotPose = limelight3.getBotPose();
-    double z = targetPose[2];
-    double x = targetPose[0];
-    double Botyaw = BotPose[5];
+    double[] BotPose = new double[7];
+    Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
+    if(alliance.get() == DriverStation.Alliance.Red){
+       BotPose = limelight3.getBotPoseRed();
+    }
+    else{
+      BotPose = limelight3.getBotPoseBlue();
+    }
+    double botx = BotPose[0];
+    double botz = BotPose[2];
     double tx = limelight3.getTx();
+    double tagId = BotPose[7];
     if(limelight3.getV() == true){
-      if(Botyaw > 0 && Botyaw <=90){
-        drivetrain.setControl(drive.
-      withRotationalRate(tx/20).
-      withVelocityX(MathUtil.clamp(x/4, -1, 1)).
-      withVelocityY(MathUtil.clamp(z/4, -1, 1)));
-      }
-      else if(Botyaw >90 && Botyaw <=180){
-      drivetrain.setControl(drive.
-      withRotationalRate(tx/20).
-      withVelocityX(MathUtil.clamp(-x/4, -1, 1)).
-      withVelocityY(MathUtil.clamp(z/4, -1, 1)));
-      }
-      else if(Botyaw >-90 && Botyaw <=0){
+      if(tagId ==8){
         drivetrain.setControl(drive.
         withRotationalRate(tx/20).
-        withVelocityX(MathUtil.clamp(-x/4, -1, 1)).
-        withVelocityY(MathUtil.clamp(-z/4, -1, 1)));
-        }
-        else if(Botyaw >-180 && Botyaw <=-90){
-          drivetrain.setControl(drive.
-          withRotationalRate(tx/20).
-          withVelocityX(MathUtil.clamp(x/4, -1, 1)).
-          withVelocityY(MathUtil.clamp(-z/4, -1, 1)));
-          }
-    }
+        withVelocityX(2-botx).
+        withVelocityY(2-botz));
+      }
+  }
     else{
       drivetrain.setControl(drive.
       withRotationalRate(0).
diff --git a/src/main/java/frc/robot/subsystems/Limelight3.java b/src/main/java/frc/robot/subsystems/Limelight3.java
index 071886f..d947aec 100644
--- a/src/main/java/frc/robot/subsystems/Limelight3.java
+++ b/src/main/java/frc/robot/subsystems/Limelight3.java
@@ -26,10 +26,16 @@ public class Limelight3 extends SubsystemBase {
   double[] targetPose = targetPoseEntry.getDoubleArray(new double[6]);
   return targetPose;
   }
-  public double[] getBotPose(){
+  public double[] getBotPoseBlue(){
     NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight");
-  NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_cameraspace");
-  double[] BotPose = BotPoseEntry.getDoubleArray(new double[6]);
+  NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue");
+  double[] BotPose = BotPoseEntry.getDoubleArray(new double[8]);
+  return BotPose;
+  }
+  public double[] getBotPoseRed(){
+    NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight");
+  NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired");
+  double[] BotPose = BotPoseEntry.getDoubleArray(new double[8]);
   return BotPose;
   }
   public double getTx(){

From 22271c8106253f5472d127e0cecb0c2960196f98 Mon Sep 17 00:00:00 2001
From: Samuel <2540638@etudiant.cegepvicto.com>
Date: Mon, 17 Nov 2025 18:13:01 -0500
Subject: [PATCH 2/2] petit modif

---
 src/main/java/frc/robot/subsystems/Limelight3.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Limelight3.java b/src/main/java/frc/robot/subsystems/Limelight3.java
index d947aec..a196fcf 100644
--- a/src/main/java/frc/robot/subsystems/Limelight3.java
+++ b/src/main/java/frc/robot/subsystems/Limelight3.java
@@ -29,13 +29,13 @@ public class Limelight3 extends SubsystemBase {
   public double[] getBotPoseBlue(){
     NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight");
   NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpiblue");
-  double[] BotPose = BotPoseEntry.getDoubleArray(new double[8]);
+  double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
   return BotPose;
   }
   public double[] getBotPoseRed(){
     NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight");
   NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_wpired");
-  double[] BotPose = BotPoseEntry.getDoubleArray(new double[8]);
+  double[] BotPose = BotPoseEntry.getDoubleArray(new double[7]);
   return BotPose;
   }
   public double getTx(){