From 083288d3fc4eb526356d0dd6287348e23231e5e9 Mon Sep 17 00:00:00 2001
From: Antoine PerreaultE <KalcioGx53@hotmail.com>
Date: Tue, 24 Mar 2026 20:20:02 -0400
Subject: [PATCH] oui

---
 src/main/java/frc/robot/RobotContainer.java   |  2 +-
 .../commands/ModeAuto/RetourMilieuDroite.java | 47 ++++++++++++++++++-
 2 files changed, 46 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 2baa8ef..f7ef44f 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -77,7 +77,7 @@ public class RobotContainer {
     NamedCommands.registerCommand("GrimperMur", new GrimperMur(limeLight3,drivetrain));
     NamedCommands.registerCommand("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain));
     NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G));
-    NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite());
+    NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite(drivetrain,limeLight3G));
     NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche());
     NamedCommands.registerCommand("Limelighter", new Limelighter(limeLight3G,drivetrain));
     NamedCommands.registerCommand("DescendreBalayeuse", new DescendreBalyeuse(balayeuse));
diff --git a/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java b/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java
index b01e9f4..9e00296 100644
--- a/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java
+++ b/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java
@@ -4,12 +4,42 @@
 
 package frc.robot.commands.ModeAuto;
 
+import static edu.wpi.first.units.Units.*;
+
+import java.util.Optional;
+
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.Limelight3G;
 
 /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
 public class RetourMilieuDroite extends Command {
+  CommandSwerveDrivetrain drivetrain;
+  Limelight3G limelight3g;
+  double botx;
+  double boty;
+  double x;
+  double y;
+  double angle;
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+  Pigeon2 pigeon2;
+  Optional<Alliance> alliance = DriverStation.getAlliance(); 
+  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
   /** Creates a new RetourMilieu. */
-  public RetourMilieuDroite() {
+  public RetourMilieuDroite(CommandSwerveDrivetrain drivetrain, Limelight3G limelight3g) {
+    this.drivetrain = drivetrain;
+    this.limelight3g = limelight3g;
+    addRequirements(drivetrain, limelight3g);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -19,7 +49,20 @@ public class RetourMilieuDroite extends Command {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    if(alliance.get() == Alliance.Blue){
+      angle = 0;
+    }
+    else{
+      angle = 180;
+    }
+    if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){
+      drivetrain.setControl(drive.withRotationalRate(0.5));
+    }
+    else if(pigeon2.getYaw().getValueAsDouble() >180){
+      drivetrain.setControl(drive.withRotationalRate(-0.5));
+    }
+  }
 
   // Called once the command ends or is interrupted.
   @Override