Merge branch 'main' of https://git.demerso.net/sdesharnais/Rebuilt-2026

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){

src/main/java/frc/robot/commands/ModeAuto/TournerA180.java

@@ -4,12 +4,28 @@
 
 package frc.robot.commands.ModeAuto;
 
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import static edu.wpi.first.units.Units.*;
 
 /* 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 TournerA180 extends Command {
-  /** Creates a new TournerA180. */
-  public TournerA180() {
+  CommandSwerveDrivetrain drivetrain;
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+  Pigeon2 pigeon2; 
+  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+  /** Creates a new TournerAZero. */
+  public TournerA180(CommandSwerveDrivetrain drivetrain) {
+    this.drivetrain = drivetrain;
+    addRequirements(drivetrain);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -19,7 +35,14 @@ public class TournerA180 extends Command {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    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
@@ -28,6 +51,6 @@ public class TournerA180 extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return pigeon2.getYaw().getValueAsDouble() > 165  && pigeon2.getYaw().getValueAsDouble() < 195;
   }
 }

src/main/java/frc/robot/commands/ModeAuto/TournerAZero.java

@@ -4,12 +4,28 @@
 
 package frc.robot.commands.ModeAuto;
 
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import static edu.wpi.first.units.Units.*;
 
 /* 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 TournerAZero extends Command {
+  CommandSwerveDrivetrain drivetrain;
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+  Pigeon2 pigeon2; 
+  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
   /** Creates a new TournerAZero. */
-  public TournerAZero() {
+  public TournerAZero(CommandSwerveDrivetrain drivetrain) {
+    this.drivetrain = drivetrain;
+    addRequirements(drivetrain);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -19,7 +35,14 @@ public class TournerAZero extends Command {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    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
@@ -28,6 +51,6 @@ public class TournerAZero extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return pigeon2.getYaw().getValueAsDouble() > 345 || pigeon2.getYaw().getValueAsDouble() < 15;
   }
 }