diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java index 401094e..81d297c 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((2-limelight3.derive(botx))). - withVelocityY(2-limelight3.derive(botz))); + withVelocityX(2-botx). + withVelocityY(2-boty)); } } else{ diff --git a/src/main/java/frc/robot/subsystems/Limelight3.java b/src/main/java/frc/robot/subsystems/Limelight3.java index 5757445..17517b6 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3.java +++ b/src/main/java/frc/robot/subsystems/Limelight3.java @@ -22,19 +22,6 @@ public class Limelight3 extends SubsystemBase { 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");