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

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).
+        withVelocityX((2-limelight3.derive(botx))).
-        withVelocityY(2-botz));
+        withVelocityY(2-limelight3.derive(botz)));
       }
   }
     else{

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,24 @@ 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-balon");
   NetworkTableEntry targetPoseEntry = limelightTable.getEntry("targetpose_cameraspace");