From 820d7de3068e62c1d984e55ec6eea0436f969d27 Mon Sep 17 00:00:00 2001
From: Samuel <2540638@etudiant.cegepvicto.com>
Date: Wed, 12 Nov 2025 18:56:01 -0500
Subject: [PATCH] limelight s'anligne test

---
 .../robot/commands/Limelight/AprilTag3.java   | 36 +++++++++++++++----
 .../java/frc/robot/subsystems/Limelight3.java | 12 +++++++
 2 files changed, 42 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java
index 32914c1..78ad1da 100644
--- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java
+++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java
@@ -10,6 +10,7 @@ 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.wpilibj2.command.Command;
 import frc.robot.TunerConstants.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
@@ -47,14 +48,37 @@ 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 b = 1.46 / Math.sqrt((limelight3.getTA()/100) / (Math.cos(90-limelight3.getTx())));
+    double[] targetPose = limelight3.getTargetPose();
+    double[] BotPose = limelight3.getBotPose();
+    double z = targetPose[2];
+    double x = targetPose[0];
+    double Botyaw = BotPose[5];
+    double tx = limelight3.getTx();
     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(a/20).
-      withVelocityX(b/50).
-      withVelocityY( 0));
-       System.out.println(b/50);
+      withRotationalRate(tx/20).
+      withVelocityX(MathUtil.clamp(-x/4, -1, 1)).
+      withVelocityY(MathUtil.clamp(z/4, -1, 1)));
+      }
+      else if(Botyaw >-90 && Botyaw <=0){
+        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)));
+          }
     }
     else{
       drivetrain.setControl(drive.
diff --git a/src/main/java/frc/robot/subsystems/Limelight3.java b/src/main/java/frc/robot/subsystems/Limelight3.java
index 75dd7ce..071886f 100644
--- a/src/main/java/frc/robot/subsystems/Limelight3.java
+++ b/src/main/java/frc/robot/subsystems/Limelight3.java
@@ -20,6 +20,18 @@ public class Limelight3 extends SubsystemBase {
       PortForwarder.add(port, "limelight.local", port);
     }
   }
+  public double[] getTargetPose(){
+    NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight");
+  NetworkTableEntry targetPoseEntry = limelightTable.getEntry("targetpose_cameraspace");
+  double[] targetPose = targetPoseEntry.getDoubleArray(new double[6]);
+  return targetPose;
+  }
+  public double[] getBotPose(){
+    NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight");
+  NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_cameraspace");
+  double[] BotPose = BotPoseEntry.getDoubleArray(new double[6]);
+  return BotPose;
+  }
   public double getTx(){
     NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
     NetworkTableEntry tx = table.getEntry("tx");