mode auto pls marche pls

src/main/java/frc/robot/commands/ModeAuto/RetourMilieuGauche.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 RetourMilieuGauche extends Command {
-  /** Creates a new RetourMilieuGauche. */
-  public RetourMilieuGauche() {
+  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 RetourMilieuGauche(CommandSwerveDrivetrain drivetrain, Limelight3G limelight3g) {
+    this.drivetrain = drivetrain;
+    this.limelight3g = limelight3g;
+    addRequirements(drivetrain, limelight3g);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -19,15 +49,85 @@ public class RetourMilieuGauche extends Command {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    if(angle < 0){
+      angle = angle + 360;
+    }
+    double[] BotPose = new double[6];
+    BotPose = limelight3g.getBotPoseBlue();
+    botx = BotPose[0];
+    boty = BotPose[1];
+    
+    if(alliance.get() == Alliance.Blue){
+      y = 7.380;
+      x = 2.305;
+      angle = 0;
+      if(limelight3g.getV()){
+        if(pigeon2.getYaw().getValueAsDouble() >355 || pigeon2.getYaw().getValueAsDouble() < 5){
+        if((y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05)){
+          drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0));
+        }
+        else{
+            if(botx < 6){
+              drivetrain.setControl(drive.withVelocityX(y-boty));
+            }
+            else{
+              drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx));
+            }
+        }
+      }
+      else{
+        if(pigeon2.getYaw().getValueAsDouble() >angle && pigeon2.getYaw().getValueAsDouble() <angle +180){
+          drivetrain.setControl(drive.withRotationalRate(0.5));
+        }
+        else if(pigeon2.getYaw().getValueAsDouble() >=angle +180){
+    
+          drivetrain.setControl(drive.withRotationalRate(-0.5));
+        }
+       }
+      }
+     }
+     else{
+      y = 0.639;
+      x = 13.963;
+      angle = 180;
+      if(limelight3g.getV()){
+        if(pigeon2.getYaw().getValueAsDouble() >175 && pigeon2.getYaw().getValueAsDouble() < 185){
+        if((y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05)){
+          drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0));
+        }
+        else{
+            if(botx > 10){
+              drivetrain.setControl(drive.withVelocityX(y-boty));
+            }
+            else{
+              drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx));
+            }
+        }
+      }
+      else{
+        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
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0));
+  }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05);
   }
 }