From 38a0fc3413c74bcd4c9d1ae0f3d135750ce001a2 Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Mon, 10 Nov 2025 18:06:08 -0500 Subject: [PATCH] avance --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/commands/Limelight/AprilTag3.java | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e0bf37a..73075ed 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -151,7 +151,7 @@ public class RobotContainer { elevateur.setDefaultCommand(new RunCommand(()->{ elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY()*manette2.getLeftY()*manette2.getLeftY(), 0.05)); }, elevateur)); - + //Reset encodeur manette2.start().whileTrue(new reset(elevateur, pince, requin)); } diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java index e0de605..920b8a7 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java @@ -50,11 +50,14 @@ public class AprilTag3 extends Command { double a = limelight3.getTx(); double b = (limelight3.getTA()*100) / (Math.cos(90-limelight3.getTx())); if(limelight3.getV() == true){ + if(limelight3.getTA() > 1.2){ drivetrain.setControl(drive. withRotationalRate(a/20). withVelocityX(0). withVelocityY( b/50)); System.out.println(b/50); + } + } else{ drivetrain.setControl(drive.