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

mode auto pls marche pls
2026-03-26 17:49:01 -04:00 · 2026-03-26 17:46:22 -04:00
8 changed files with 246 additions and 24 deletions
--- 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("GrimperReservoir", new GrimperReservoir(limeLight3G,drivetrain));
    NamedCommands.registerCommand("Lancer", new LancerAuto(lanceur, limeLight3G));
    NamedCommands.registerCommand("RetourMilieuDroite", new RetourMilieuDroite(drivetrain,limeLight3G));
-    NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche());
+    NamedCommands.registerCommand("RetourMilieuGauche", new RetourMilieuGauche(drivetrain, limeLight3G));
    NamedCommands.registerCommand("Limelighter", new Limelighter(limeLight3G,drivetrain));
    NamedCommands.registerCommand("DescendreBalayeuse", new DescendreBalyeuse(balayeuse));
    NamedCommands.registerCommand("Aspirer", new AspirerAuto(balayeuse));
--- a/src/main/java/frc/robot/commands/Limelighter.java
+++ b/src/main/java/frc/robot/commands/Limelighter.java
@@ -7,12 +7,16 @@ package frc.robot.commands;
 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;
 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 Limelighter extends Command {
  Limelight3G limelight3g;
@@ -23,6 +27,7 @@ public class Limelighter extends Command {
  double boty;
  double angle;
  double calcul;
+   Optional<Alliance> alliance = DriverStation.getAlliance(); 
  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
      .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1)
      .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
@@ -45,7 +50,13 @@ public class Limelighter extends Command {
    double[] BotPose = new double[6];
    System.out.println("e");
    if (limelight3g.getV()) {
-      BotPose = limelight3g.getBotPoseBlue();
+      if(alliance.get() == Alliance.Blue){
+        BotPose = limelight3g.getBotPoseBlue();
+      }
+      else{
+        BotPose = limelight3g.getBotPoseRed();
+      }
+      
      botx = BotPose[1];
      boty = BotPose[0];
      angle = drivetrain.getPigeon2().getYaw().getValueAsDouble();
--- a/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java
+++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperMur.java
@@ -8,10 +8,14 @@ import static edu.wpi.first.units.Units.MetersPerSecond;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.RotationsPerSecond;

+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;
@@ -24,9 +28,13 @@ public class GrimperMur extends Command {
  double[] BotPose = new double[6];
  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);
@@ -48,12 +56,42 @@ public class GrimperMur extends Command {
    BotPose = limeLight3.getBotPoseBlue();
    botx = BotPose[0];
    boty = BotPose[1];
-    if(180-pigeon2.getYaw().getValueAsDouble() < 10 && 180- pigeon2.getYaw().getValueAsDouble() > -10){
-      drivetrain.setControl(drive.withVelocityX(2.961328-boty).withVelocityY(1.11-botx));
+    if(angle < 0){
+      angle = angle + 360;
+    }
+    if(alliance.get() == Alliance.Blue){
+     y = 2.961328;
+     x = 1.11;
+     angle = 0;
+     if(pigeon2.getYaw().getValueAsDouble() > 355 || pigeon2.getYaw().getValueAsDouble() < 5){
+      drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx));
    }
    else{
-       drivetrain.setControl(drive.withRotationalRate(180-pigeon2.getYaw().getValueAsDouble()/45));
+      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));
+      }
    }
+    }
+    else{
+     x = 11.45966;
+     y = 6.959326;
+     angle = 180;
+     if(pigeon2.getYaw().getValueAsDouble() > 175 && pigeon2.getYaw().getValueAsDouble() < 185){
+      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.
@@ -65,6 +103,6 @@ public class GrimperMur extends Command {
  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
-    return (2.961328-boty < 0.05 || 2.961328-boty >-0.05) && (1.11-botx < 0.05 || 1.11-botx > -0.05);
+    return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05);
  }
-}
+}
--- a/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java
+++ b/src/main/java/frc/robot/commands/ModeAuto/GrimperReservoir.java
@@ -8,10 +8,14 @@ import static edu.wpi.first.units.Units.MetersPerSecond;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.RotationsPerSecond;

+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;
@@ -24,9 +28,13 @@ public class GrimperReservoir extends Command {
  double[] BotPose = new double[6];
  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);
@@ -48,12 +56,42 @@ public class GrimperReservoir extends Command {
    BotPose = limeLight3G.getBotPoseBlue();
    botx = BotPose[0];
    boty = BotPose[1];
-    if(0-pigeon2.getYaw().getValueAsDouble() < 10 && 0- pigeon2.getYaw().getValueAsDouble() > -10){
-      drivetrain.setControl(drive.withVelocityX(5.081328-boty).withVelocityY(1.11-botx));
+    if(angle < 0){
+      angle = angle + 360;
+    }
+    if(alliance.get() == Alliance.Blue){
+     y = 5.081328;
+     x = 1.11;
+     angle = 180;
+     if(pigeon2.getYaw().getValueAsDouble() < 185 && pigeon2.getYaw().getValueAsDouble() > 175){
+      drivetrain.setControl(drive.withVelocityX(y-boty).withVelocityY(x-botx));
    }
    else{
-       drivetrain.setControl(drive.withRotationalRate(0-pigeon2.getYaw().getValueAsDouble()/45));
+      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));
+      }
    }
+    }
+    else{
+     y = 6.959326;
+     x = 13.57966;
+     angle = 0;
+     if(pigeon2.getYaw().getValueAsDouble() > 355 || pigeon2.getYaw().getValueAsDouble() < 5){
+      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.
@@ -65,6 +103,6 @@ public class GrimperReservoir extends Command {
  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
-    return 5.081328-boty < 0.05 || 5.081328-boty >-0.05 && 1.11-botx < 0.05 || 1.11-botx > -0.05;
+    return (y-boty < 0.05 && y-boty >-0.05) && (x-botx < 0.05 && x-botx > -0.05);
  }
-}
+}
--- a/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java
+++ b/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuDroite.java
@@ -50,11 +50,43 @@ public class RetourMilieuDroite extends Command {
  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
-    if(alliance.get() == Alliance.Blue){
-      angle = 0;
+    if(angle < 0){
+      angle = angle + 360;
    }
-    else{
+    if(alliance.get() == Alliance.Blue){
+      y = 0.639;
+      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 = 7.380;
+      x = 13.963;
      angle = 180;
+      if(limelight3g.getV()){
    }
    if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){
      drivetrain.setControl(drive.withRotationalRate(0.5));
@@ -63,14 +95,17 @@ public class RetourMilieuDroite extends Command {
      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);
  }
 }
--- a/src/main/java/frc/robot/commands/ModeAuto/RetourMilieuGauche.java
+++ b/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);
  }
 }
--- a/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java
+++ b/src/main/java/frc/robot/commands/ModeAuto/TournerVersMur.java
@@ -66,6 +66,6 @@ public class TournerVersMur extends Command {
  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
-    return pigeon2.getYaw().getValueAsDouble() > angle - 5  && pigeon2.getYaw().getValueAsDouble() < angle + 5;
+    return pigeon2.getYaw().getValueAsDouble() > angle && pigeon2.getYaw().getValueAsDouble() < angle + 5;
  }
 }
--- a/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java
+++ b/src/main/java/frc/robot/commands/ModeAuto/TournerVersReservoir.java
@@ -66,6 +66,6 @@ public class TournerVersReservoir extends Command {
  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
-    return  pigeon2.getYaw().getValueAsDouble() > angle - 5   && pigeon2.getYaw().getValueAsDouble() < angle + 5;
+    return  pigeon2.getYaw().getValueAsDouble() > angle && pigeon2.getYaw().getValueAsDouble() < angle + 5;
  }
 }
Author	SHA1	Message	Date
Antoine PerreaultE	5ba5cce953	Merge branch 'main' of https://git.demerso.net/sdesharnais/Rebuilt-2026	2026-03-26 17:49:01 -04:00
Antoine PerreaultE	d0c50cbd6e	mode auto pls marche pls	2026-03-26 17:46:22 -04:00