diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java index d95a6a8..401094e 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java @@ -74,8 +74,8 @@ public class AprilTag3 extends Command { else if(tagId ==7){ drivetrain.setControl(drive. withRotationalRate(tx/20). - withVelocityX(-(botx-2.21)*2). - withVelocityY(-(boty-4)*4)); + 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 27a9a37..5757445 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,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");