From 4068016d36d1f4e2b0c8cb32c968d8c2b9f5d138 Mon Sep 17 00:00:00 2001 From: odubois Date: Tue, 31 Mar 2026 03:31:15 +0200 Subject: [PATCH 1/2] get alliance -> dans initialise --- src/main/java/frc/robot/commands/Lancer.java | 3 ++- src/main/java/frc/robot/commands/Limelighter.java | 4 +++- src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java | 4 +++- .../java/frc/robot/commands/ModeAuto/GrimperReservoir.java | 4 +++- src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java | 1 + .../java/frc/robot/commands/ModeAuto/LimelighterAuto.java | 4 +++- src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java | 4 +++- .../frc/robot/commands/ModeAuto/TournerVersReservoir.java | 4 +++- 8 files changed, 21 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/commands/Lancer.java b/src/main/java/frc/robot/commands/Lancer.java index f1df279..e29f8b1 100644 --- a/src/main/java/frc/robot/commands/Lancer.java +++ b/src/main/java/frc/robot/commands/Lancer.java @@ -39,7 +39,8 @@ public class Lancer extends Command { public void initialize() { if(limeLight3G.getV()){} pidController = new PIDController(0.0007, 0,0, 0.001); - timer.reset(); + timer.reset(); + alliance = DriverStation.getAlliance(); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/Limelighter.java b/src/main/java/frc/robot/commands/Limelighter.java index ce3c27f..f53e67c 100644 --- a/src/main/java/frc/robot/commands/Limelighter.java +++ b/src/main/java/frc/robot/commands/Limelighter.java @@ -42,7 +42,9 @@ public class Limelighter extends Command { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + alliance = DriverStation.getAlliance(); + } // Called every time the scheduler runs while the command is scheduled. @Override diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java index 768028c..c0bcfa2 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java @@ -47,7 +47,9 @@ public class GrimperMur extends Command { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + alliance = DriverStation.getAlliance(); + } // Called every time the scheduler runs while the command is scheduled. @Override diff --git a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java index 381b213..8e13cd8 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java @@ -47,7 +47,9 @@ public class GrimperReservoir extends Command { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + alliance = DriverStation.getAlliance(); + } // Called every time the scheduler runs while the command is scheduled. @Override diff --git a/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java index 2205a84..6dfb2b2 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java @@ -38,6 +38,7 @@ public class LancerAuto extends Command { public void initialize() { pidController = new PIDController(0.0007, 0, 0, 0.001); timer.reset(); + alliance = DriverStation.getAlliance(); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java index 3e749da..71fbab5 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java @@ -42,7 +42,9 @@ public class LimelighterAuto extends Command { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + alliance = DriverStation.getAlliance(); + } // Called every time the scheduler runs while the command is scheduled. @Override diff --git a/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java b/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java index 24249d7..331f796 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java +++ b/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java @@ -37,7 +37,9 @@ public class TournerVersMur extends Command { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + alliance = DriverStation.getAlliance(); + } // Called every time the scheduler runs while the command is scheduled. @Override diff --git a/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java b/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java index 0e537eb..c2b9232 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java +++ b/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java @@ -37,7 +37,9 @@ public class TournerVersReservoir extends Command { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + alliance = DriverStation.getAlliance(); + } // Called every time the scheduler runs while the command is scheduled. @Override From 098f16665a5ad1c94200de52b79b65a979ac7a4a Mon Sep 17 00:00:00 2001 From: odubois Date: Tue, 31 Mar 2026 05:19:16 +0200 Subject: [PATCH 2/2] limelight pls marche --- .../java/frc/robot/commands/Limelighter.java | 59 +++++++++++------ .../commands/ModeAuto/LimelighterAuto.java | 64 ++++++++++++------- .../frc/robot/subsystems/Limelight3G.java | 32 ++++++---- 3 files changed, 97 insertions(+), 58 deletions(-) diff --git a/src/main/java/frc/robot/commands/Limelighter.java b/src/main/java/frc/robot/commands/Limelighter.java index 2778a65..ed0933b 100644 --- a/src/main/java/frc/robot/commands/Limelighter.java +++ b/src/main/java/frc/robot/commands/Limelighter.java @@ -53,46 +53,63 @@ public class Limelighter extends Command { double[] BotPose = new double[6]; System.out.println("e"); if (limelight3g.getV()) { - BotPose = limelight3g.getBotPoseBlue(); + // BotPose = limelight3g.getBotPoseBlue(); if (!alliance.isPresent()) { return; } if (alliance.get() == Alliance.Blue) { x = 4.6; + BotPose = limelight3g.getBotPoseBlue(); } else { x = 11.915394; + BotPose = limelight3g.getBotPoseRed(); } botx = BotPose[1]; boty = BotPose[0]; angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); calcul = limelight3g.Calcule(botx, x, boty, 4, angle); - if(calcul < -5 && calcul > -180){ - drivetrain.setControl( - drive.withRotationalRate(0.5*(2*Math.PI))); + if(angle > 180){ + angle -= 360; } - else if(calcul > 5 && calcul < 180){ - drivetrain.setControl( - drive.withRotationalRate(-0.5*(2*Math.PI))); + if(calcul > -5 && calcul < 5){ + drivetrain.setControl( + drive.withRotationalRate(0)); } - else if(calcul >= 180){ + else if(calcul > 5){ drivetrain.setControl( - drive.withRotationalRate(-0.5*(2*Math.PI))); + drive.withRotationalRate(-0.5*(2*Math.PI))); } - else if(calcul <= -180){ + else if(calcul < -5){ drivetrain.setControl( - drive.withRotationalRate(0.5*(2*Math.PI))); - } - else{ - drivetrain.setControl( - drive.withRotationalRate(0)); - } - drivetrain.setControl( - drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); - System.out.println(angle); - if (calcul < 0.2 && calcul > -0.2) { - drivetrain.setControl(drive.withRotationalRate(0)); + drive.withRotationalRate(-0.5*(2*Math.PI))); } + // if(calcul < -5 && calcul > -180){ + // drivetrain.setControl( + // drive.withRotationalRate(0.5*(2*Math.PI))); + // } + // else if(calcul > 5 && calcul < 180){ + // drivetrain.setControl( + // drive.withRotationalRate(-0.5*(2*Math.PI))); + // } + // else if(calcul >= 180){ + // drivetrain.setControl( + // drive.withRotationalRate(-0.5*(2*Math.PI))); + // } + // else if(calcul <= -180){ + // drivetrain.setControl( + // drive.withRotationalRate(0.5*(2*Math.PI))); + // } + // else{ + // drivetrain.setControl( + // drive.withRotationalRate(0)); + // } + // drivetrain.setControl( + // drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); + // System.out.println(angle); + // if (calcul < 0.2 && calcul > -0.2) { + // drivetrain.setControl(drive.withRotationalRate(0)); + // } } else{ drivetrain.setControl(drive.withRotationalRate(0)); diff --git a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java index 0a60359..21cb1e8 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LimelighterAuto.java @@ -53,46 +53,63 @@ public class LimelighterAuto extends Command { double[] BotPose = new double[6]; System.out.println("e"); if (limelight3g.getV()) { - if (!alliance.isPresent()) { + // BotPose = limelight3g.getBotPoseBlue(); + if (!alliance.isPresent()) { return; } if (alliance.get() == Alliance.Blue) { x = 4.6; + BotPose = limelight3g.getBotPoseBlue(); } else { x = 11.915394; + BotPose = limelight3g.getBotPoseRed(); } - BotPose = limelight3g.getBotPoseBlue(); botx = BotPose[1]; boty = BotPose[0]; angle = drivetrain.getPigeon2().getYaw().getValueAsDouble(); calcul = limelight3g.Calcule(botx, x, boty, 4, angle); - if(calcul < -5 && calcul > -180){ - drivetrain.setControl( - drive.withRotationalRate(0.5*(2*Math.PI))); + if(angle > 180){ + angle -= 360; } - else if(calcul > 5 && calcul < 180){ - drivetrain.setControl( - drive.withRotationalRate(-0.5*(2*Math.PI))); + if(calcul > -5 && calcul < 5){ + drivetrain.setControl( + drive.withRotationalRate(0)); } - else if(calcul >= 180){ + else if(calcul > 5){ drivetrain.setControl( - drive.withRotationalRate(-0.5*(2*Math.PI))); + drive.withRotationalRate(-0.5*(2*Math.PI))); } - else if(calcul <= -180){ + else if(calcul < -5){ drivetrain.setControl( - drive.withRotationalRate(0.5*(2*Math.PI))); - } - else{ - drivetrain.setControl( - drive.withRotationalRate(0)); - } - drivetrain.setControl( - drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); - System.out.println(angle); - if (calcul < 0.2 && calcul > -0.2) { - drivetrain.setControl(drive.withRotationalRate(0)); + drive.withRotationalRate(-0.5*(2*Math.PI))); } + // if(calcul < -5 && calcul > -180){ + // drivetrain.setControl( + // drive.withRotationalRate(0.5*(2*Math.PI))); + // } + // else if(calcul > 5 && calcul < 180){ + // drivetrain.setControl( + // drive.withRotationalRate(-0.5*(2*Math.PI))); + // } + // else if(calcul >= 180){ + // drivetrain.setControl( + // drive.withRotationalRate(-0.5*(2*Math.PI))); + // } + // else if(calcul <= -180){ + // drivetrain.setControl( + // drive.withRotationalRate(0.5*(2*Math.PI))); + // } + // else{ + // drivetrain.setControl( + // drive.withRotationalRate(0)); + // } + // drivetrain.setControl( + // drive.withVelocityX(0).withVelocityY(0).withRotationalRate(calcul)); + // System.out.println(angle); + // if (calcul < 0.2 && calcul > -0.2) { + // drivetrain.setControl(drive.withRotationalRate(0)); + // } } else{ drivetrain.setControl(drive.withRotationalRate(0)); @@ -109,7 +126,6 @@ public class LimelighterAuto extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return calcul < 5 && calcul > -5; + return calcul > -5 && calcul < 5; } } - diff --git a/src/main/java/frc/robot/subsystems/Limelight3G.java b/src/main/java/frc/robot/subsystems/Limelight3G.java index da4ce72..5bc3bb6 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3G.java +++ b/src/main/java/frc/robot/subsystems/Limelight3G.java @@ -52,22 +52,28 @@ NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-tag") } public double Calcule(double x1, double x2, double y1, double y2, double angle) { - if(x1 > x2){ - if(y1 > y2){ - return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI) - angle; + // if(x1 > x2){ + // if(y1 > y2){ + // return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI) - angle; + // } + // else{ + // return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+90 - angle; + // } + // } + // else{ + // if(y1 > y2){ + // return Math.atan((x2 - x1) / (y2 - y1))* (180 / Math.PI)+270 - angle; + // } + // else{ + // return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI)+180 - angle; + // } + // } + if(y1 > y2){ + return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI) - angle; } else{ - return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)+90 - angle; + return Math.atan((x2 - x1) / (y2 - y1)) * (180 / Math.PI)-360 - angle; } - } - else{ - if(y1 > y2){ - return Math.atan((x2 - x1) / (y2 - y1))* (180 / Math.PI)+270 - angle; - } - else{ - return Math.atan(90-((x2 - x1) / (y2 - y1))) * (180 / Math.PI)+180 - angle; - } - } } @Override public void periodic() {