tourner a zero/180 regle

src/main/java/frc/robot/RobotContainer.java

@@ -38,6 +38,7 @@ import frc.robot.commands.ModeAuto.LancerAuto;
 import frc.robot.commands.ModeAuto.RetourMilieuDroite;
 import frc.robot.commands.ModeAuto.RetourMilieuGauche;
 import frc.robot.commands.ModeAuto.TournerVersMur;
+import frc.robot.commands.ModeAuto.TournerVersReservoir;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.Balayeuse;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
@@ -81,7 +82,8 @@ public class RobotContainer {
     NamedCommands.registerCommand("Limelighter", new Limelighter(limeLight3G,drivetrain));
     NamedCommands.registerCommand("DescendreBalayeuse", new DescendreBalyeuse(balayeuse));
     NamedCommands.registerCommand("Aspirer", new AspirerAuto(balayeuse));
-    NamedCommands.registerCommand("TournerA180", new TournerVersMur(drivetrain));
+    NamedCommands.registerCommand("TournerA180", new TournerVersReservoir(drivetrain));
+    NamedCommands.registerCommand("TournerAZero", new TournerVersMur(drivetrain));
     NamedCommands.registerCommand("MonterGrimpeur", new MonterGrimpeur(grimpeur));
     NamedCommands.registerCommand("DescendreGrimpeur", new DescendreGrimpeur(grimpeur));
     autoChooser = AutoBuilder.buildAutoChooser();

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

@@ -25,6 +25,7 @@ public class TournerVersMur extends Command {
   private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
   Pigeon2 pigeon2; 
   double force;
+  double angle;
   private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
             .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
             .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
@@ -44,9 +45,11 @@ public class TournerVersMur extends Command {
   public void execute() {
     if(alliance.get() == Alliance.Blue){
       force = 0.5;
+      angle = 0;
     }
     else{
       force = -0.5;
+      angle = 180;
     }
     if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){
       drivetrain.setControl(drive.withRotationalRate(force));
@@ -63,6 +66,6 @@ public class TournerVersMur extends Command {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return pigeon2.getYaw().getValueAsDouble() > 165  && pigeon2.getYaw().getValueAsDouble() < 195;
+    return pigeon2.getYaw().getValueAsDouble() > angle - 5  && pigeon2.getYaw().getValueAsDouble() < angle + 5;
   }
 }

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

@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+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.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 static edu.wpi.first.units.Units.*;
+
+import java.util.Optional;
+
+/* 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 TournerVersReservoir extends Command {
+  CommandSwerveDrivetrain drivetrain;
+  Optional<Alliance> alliance = DriverStation.getAlliance();
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+  Pigeon2 pigeon2; 
+  double force;
+  double angle;
+  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+            .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) 
+            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+  /** Creates a new TournerAZero. */
+  public TournerVersReservoir(CommandSwerveDrivetrain drivetrain) {
+    this.drivetrain = drivetrain;
+    addRequirements(drivetrain);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if(alliance.get() == Alliance.Blue){
+      force = 0.5;
+      angle = 180;
+    }
+    else{
+      force = -0.5;
+      angle = 0;
+    }
+    if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){
+      drivetrain.setControl(drive.withRotationalRate(-force));
+    }
+    else if(pigeon2.getYaw().getValueAsDouble() >180){
+      drivetrain.setControl(drive.withRotationalRate(force));
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return  pigeon2.getYaw().getValueAsDouble() > angle - 5   && pigeon2.getYaw().getValueAsDouble() < angle + 5;
+  }
+}