From d81ff3db3db046cc5a3c4a0b6f16c24a28c0d1f0 Mon Sep 17 00:00:00 2001
From: Antoine PerreaultE <KalcioGx53@hotmail.com>
Date: Tue, 10 Mar 2026 18:27:47 -0400
Subject: [PATCH] Lanceur re/gle/

---
 src/main/java/frc/robot/RobotContainer.java  |  4 +++-
 src/main/java/frc/robot/commands/Lancer.java | 10 ++++++++--
 2 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index ed33fb2..186277c 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -32,12 +32,14 @@ import frc.robot.subsystems.Grimpeur;
 import frc.robot.subsystems.Lanceur;
 import frc.robot.subsystems.Led;
 import frc.robot.subsystems.LimeLight3;
+import frc.robot.subsystems.Limelight3G;
 
 public class RobotContainer {
   Balayeuse balayeuse = new Balayeuse();
   Grimpeur grimpeur = new Grimpeur();
   Lanceur lanceur = new Lanceur();
   LimeLight3 limeLight3 = new LimeLight3();
+  Limelight3G limeLight3G = new Limelight3G();
   Led led = new Led();
   CommandXboxController manette = new CommandXboxController(0);
   public RobotContainer() {
@@ -58,7 +60,7 @@ public class RobotContainer {
   }
 
   private void configureBindings() {
-    manette.a().whileTrue(new Lancer(lanceur,limeLight3,balayeuse,led));
+    manette.a().whileTrue(new Lancer(lanceur,limeLight3G,balayeuse,led));
     manette.y().whileTrue(new ModeOposer(lanceur, balayeuse));
     manette.x().whileTrue(new LancerBaseVitesse(lanceur, limeLight3, balayeuse, led));
     manette.rightTrigger().whileTrue(new MonterGrimpeur(grimpeur));
diff --git a/src/main/java/frc/robot/commands/Lancer.java b/src/main/java/frc/robot/commands/Lancer.java
index cc19ef5..5a7674b 100644
--- a/src/main/java/frc/robot/commands/Lancer.java
+++ b/src/main/java/frc/robot/commands/Lancer.java
@@ -23,7 +23,7 @@ public class Lancer extends Command {
   private Led led;
   private double temp;
   /** Creates a new Lancer. */
-  public Lancer(Lanceur lanceur, LimeLight3 limeLight3, Balayeuse balayeuse,Led led) {
+  public Lancer(Lanceur lanceur, Limelight3G limeLight3G, Balayeuse balayeuse,Led led) {
     this.lanceur = lanceur;
     this.balayeuse = balayeuse;
     this.led = led;
@@ -46,6 +46,10 @@ public class Lancer extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    double[] BotPose = new double[6];
+    BotPose = limeLight3G.getBotPoseBlue();
+    double botx = BotPose[0];
+    double boty = BotPose[1];
     int nbFois = 0;
 
     double moyenneAmp = 0;
@@ -65,7 +69,9 @@ public class Lancer extends Command {
       led.Jaune2();
     }
     else{
-      double vitesse = (100-limeLight3G.getTA())/lanceur.Vitesse();
+                                                //pythagore |
+                                                //         \/
+      double vitesse = Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2));
       double output = pidController.calculate(lanceur.Vitesse(),vitesse);
       lanceur.Lancer(output);
       if(lanceur.Vitesse() >= vitesse){