diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java index 7dd2172..b275a8f 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java @@ -48,12 +48,12 @@ public class AprilTag3 extends Command { @Override public void execute() { double a = limelight3.getTx(); - double b = limelight3.getRx(); + double b = limelight3.getTA()*100/Math.cos(90-limelight3.getTx()); if(limelight3.getV() == true){ drivetrain.setControl(drive. withRotationalRate(a/10). withVelocityX(0). - withVelocityY(b/10)); + withVelocityY( b/30)); System.out.println(b/10); } else{ diff --git a/src/main/java/frc/robot/subsystems/Limelight3.java b/src/main/java/frc/robot/subsystems/Limelight3.java index 787fc37..75dd7ce 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3.java +++ b/src/main/java/frc/robot/subsystems/Limelight3.java @@ -25,6 +25,11 @@ public class Limelight3 extends SubsystemBase { NetworkTableEntry tx = table.getEntry("tx"); return tx.getDouble(0.0); } + public double getTA(){ + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon"); + NetworkTableEntry ta = table.getEntry("ta"); + return ta.getDouble(0.0); + } public double getRx(){ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon"); NetworkTableEntry rx = table.getEntry("rx");