From 3ec58dca1acbc31b7c406b8a36d05e9112fba151 Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Mon, 17 Nov 2025 18:14:43 -0500 Subject: [PATCH 1/2] --- .../java/frc/robot/commands/Limelight/AprilTag3.java | 11 ++++++----- src/main/java/frc/robot/subsystems/Limelight3.java | 6 +++--- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java index 78ad1da..df05ca0 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java @@ -55,23 +55,24 @@ public class AprilTag3 extends Command { double Botyaw = BotPose[5]; double tx = limelight3.getTx(); if(limelight3.getV() == true){ + System.out.println(Botyaw); if(Botyaw > 0 && Botyaw <=90){ drivetrain.setControl(drive. withRotationalRate(tx/20). - withVelocityX(MathUtil.clamp(x/4, -1, 1)). + withVelocityX(MathUtil.clamp(-x/4, -1, 1)). withVelocityY(MathUtil.clamp(z/4, -1, 1))); } else if(Botyaw >90 && Botyaw <=180){ drivetrain.setControl(drive. - withRotationalRate(tx/20). - withVelocityX(MathUtil.clamp(-x/4, -1, 1)). - withVelocityY(MathUtil.clamp(z/4, -1, 1))); + withRotationalRate(0). + withVelocityX(MathUtil.clamp(x/4, -1, 1)). + withVelocityY(MathUtil.clamp(-z/4, -1, 1))); } else if(Botyaw >-90 && Botyaw <=0){ drivetrain.setControl(drive. withRotationalRate(tx/20). withVelocityX(MathUtil.clamp(-x/4, -1, 1)). - withVelocityY(MathUtil.clamp(-z/4, -1, 1))); + withVelocityY(MathUtil.clamp(z/4, -1, 1))); } else if(Botyaw >-180 && Botyaw <=-90){ drivetrain.setControl(drive. diff --git a/src/main/java/frc/robot/subsystems/Limelight3.java b/src/main/java/frc/robot/subsystems/Limelight3.java index 071886f..d86abc0 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3.java +++ b/src/main/java/frc/robot/subsystems/Limelight3.java @@ -21,14 +21,14 @@ public class Limelight3 extends SubsystemBase { } } public double[] getTargetPose(){ - NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight"); + NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon"); NetworkTableEntry targetPoseEntry = limelightTable.getEntry("targetpose_cameraspace"); double[] targetPose = targetPoseEntry.getDoubleArray(new double[6]); return targetPose; } public double[] getBotPose(){ - NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight"); - NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose_cameraspace"); + NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon"); + NetworkTableEntry BotPoseEntry = limelightTable.getEntry("botpose"); double[] BotPose = BotPoseEntry.getDoubleArray(new double[6]); return BotPose; } From 2b6839b686d5bfcb5e21d4385e2292c88c7936b7 Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Mon, 17 Nov 2025 18:17:26 -0500 Subject: [PATCH 2/2] --- .../frc/robot/commands/Limelight/AprilTag3.java | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java index 2eead71..54bb595 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java @@ -67,16 +67,10 @@ public class AprilTag3 extends Command { if(tagId ==8){ drivetrain.setControl(drive. withRotationalRate(tx/20). - withVelocityX(MathUtil.clamp(-x/4, -1, 1)). - withVelocityY(MathUtil.clamp(-z/4, -1, 1))); - } - else if(Botyaw >-180 && Botyaw <=-90){ - drivetrain.setControl(drive. - withRotationalRate(tx/20). - withVelocityX(MathUtil.clamp(x/4, -1, 1)). - withVelocityY(MathUtil.clamp(-z/4, -1, 1))); - } - } + withVelocityX(2-botx). + withVelocityY(2-botz)); + } + } else{ drivetrain.setControl(drive. withRotationalRate(0).