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;