diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java index e0de605..32914c1 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.getTA()*100) / (Math.cos(90-limelight3.getTx())); + double b = 1.46 / Math.sqrt((limelight3.getTA()/100) / (Math.cos(90-limelight3.getTx()))); if(limelight3.getV() == true){ drivetrain.setControl(drive. withRotationalRate(a/20). - withVelocityX(0). - withVelocityY( b/50)); + withVelocityX(b/50). + withVelocityY( 0)); System.out.println(b/50); } else{