From 688d393d35888cf27091f0ea76cd02ef1b13fc8f Mon Sep 17 00:00:00 2001
From: Samuel <2540638@etudiant.cegepvicto.com>
Date: Thu, 20 Nov 2025 14:33:00 -0500
Subject: [PATCH] =?UTF-8?q?teste=20pente=20d'acc=C3=A9l=C3=A9ration=20d?=
 =?UTF-8?q?=C3=A9river?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../frc/robot/commands/Limelight/AprilTag3.java |  4 ++--
 .../java/frc/robot/subsystems/Limelight3.java   | 17 ++++++++++++++++-
 2 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java
index 54bb595..a517ad9 100644
--- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java
+++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java
@@ -67,8 +67,8 @@ public class AprilTag3 extends Command {
       if(tagId ==8){
         drivetrain.setControl(drive.
         withRotationalRate(tx/20).
-        withVelocityX(2-botx).
-        withVelocityY(2-botz));
+        withVelocityX((2-limelight3.derive(botx))).
+        withVelocityY(2-limelight3.derive(botz)));
       }
   }
     else{
diff --git a/src/main/java/frc/robot/subsystems/Limelight3.java b/src/main/java/frc/robot/subsystems/Limelight3.java
index a196fcf..321aa58 100644
--- a/src/main/java/frc/robot/subsystems/Limelight3.java
+++ b/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,12 +17,26 @@ 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);
     }
   }
+  double previousValue = 0;
+double previousTime = Timer.getFPGATimestamp();
+
+public double derive(double currentValue) {
+  double previousValue = 0;
+  double previousTime = Timer.getFPGATimestamp();  
+  double now = Timer.getFPGATimestamp();
+  double dt = now - previousTime;
+  double derivative = (currentValue - previousValue) / dt;
+  previousValue = currentValue;
+  previousTime = now;
+  return derivative;
+}
   public double[] getTargetPose(){
-    NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight");
+  NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight");
   NetworkTableEntry targetPoseEntry = limelightTable.getEntry("targetpose_cameraspace");
   double[] targetPose = targetPoseEntry.getDoubleArray(new double[6]);
   return targetPose;