Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025

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

@@ -26,22 +26,24 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.TunerConstants.TunerConstants;
 import frc.robot.commands.AprilTag3;
 import frc.robot.commands.AprilTag3G;
-import frc.robot.commands.Depart;
-import frc.robot.commands.ElevateurManuel;
 import frc.robot.commands.Forme3;
-import frc.robot.commands.L2;
-import frc.robot.commands.L3;
-import frc.robot.commands.L4;
-import frc.robot.commands.PinceManuel;
-import frc.robot.commands.PinceManuel2;
 import frc.robot.commands.RainBow;
 import frc.robot.commands.StationPince;
 import frc.robot.commands.reset;
+import frc.robot.commands.Elevateur.Depart;
+import frc.robot.commands.Elevateur.ElevateurManuel;
+import frc.robot.commands.Elevateur.L2;
+import frc.robot.commands.Elevateur.L3;
+import frc.robot.commands.Elevateur.L4;
 import frc.robot.commands.Pince.AlgueExpire;
 import frc.robot.commands.Pince.Algue_inspire;
 import frc.robot.commands.Pince.CorailAspir;
 import frc.robot.commands.Pince.CoralAlgueInspire;
 import frc.robot.commands.Pince.CoralExpire;
+import frc.robot.commands.Pince.PinceManuel;
+import frc.robot.commands.requin.BalayeuseBas;
+import frc.robot.commands.requin.BalayeuseHaut;
+import frc.robot.commands.requin.exspire;
 import frc.robot.subsystems.Bougie;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 import frc.robot.subsystems.Elevateur;
@@ -49,6 +51,7 @@ import frc.robot.subsystems.Grimpeur;
 import frc.robot.subsystems.Limelight3;
 import frc.robot.subsystems.Limelight3G;
 import frc.robot.subsystems.Pince;
+import frc.robot.subsystems.Requin;
 
 public class RobotContainer {
   private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
@@ -74,12 +77,12 @@ public class RobotContainer {
   Pince pince = new Pince();
   ElevateurManuel elevateurManuel = new ElevateurManuel(elevateur, manette2::getLeftY);
   PinceManuel pinceManuel = new PinceManuel(pince,manette2::getRightY);
-  PinceManuel2 pinceManuel2 = new PinceManuel2(pince);
   Bougie bougie = new Bougie();
   Limelight3G limelight3g = new Limelight3G();
   Limelight3 limelight3 = new Limelight3();
   Pose2d pose = new Pose2d();
   Grimpeur Grimpeur = new Grimpeur();
+  Requin requin = new Requin();
   
   public RobotContainer() {
     autoChooser = AutoBuilder.buildAutoChooser("New Auto");
@@ -93,58 +96,58 @@ public class RobotContainer {
     NamedCommands.registerCommand("CoraletAlgue", new CoralAlgueInspire(pince,bougie));
     }
   private void configureBindings() {
-    
+    drivetrain.registerTelemetry(logger::telemeterize);
  drivetrain.setDefaultCommand(
           // Drivetrain will execute this command periodically
           drivetrain.applyRequest(() ->
-            drive.withVelocityX(MathUtil.applyDeadband(Math.pow(-manette1.getLeftX(), 2)*MaxSpeed, 0.1)) // Drive forward with negative Y (forward)
-              .withVelocityY(MathUtil.applyDeadband(Math.pow(-manette1.getLeftY(), 2)*MaxSpeed, 0.10000)) // Drive left with negative X (left)
-              .withRotationalRate(MathUtil.applyDeadband(Math.pow(-manette1.getRightX(), 2)*manette1.getRightX()*MaxAngularRate, 0.1)) // Drive counterclockwise with negative X (left)
+            drive.withVelocityX(MathUtil.applyDeadband(-manette1.getLeftX()*manette1.getLeftX()*manette1.getLeftX()*MaxSpeed, 0.05)) // Drive forward with negative Y (forward)
+              .withVelocityY(MathUtil.applyDeadband(manette1.getLeftY()*manette1.getLeftY()*manette1.getLeftY()*MaxSpeed, 0.05)) // Drive left with negative X (left)
+              .withRotationalRate(MathUtil.applyDeadband(-manette1.getRightX()*manette1.getRightX()*manette1.getRightX()*MaxAngularRate, 0.05)) // Drive counterclockwise with negative X (left)
         )
       );
-    // Elevateur manuel
-    drivetrain.registerTelemetry(logger::telemeterize);
-    elevateur.setDefaultCommand(new RunCommand(()->{
-      elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY(), 0.1));
-    }, elevateur));
-
-    //Pince manuel
-    pince.setDefaultCommand(new RunCommand(()->{
-      pince.pivote(MathUtil.applyDeadband(manette2.getRightY(), 0.1));
-    }, pince));
+   
      
-    
-    // manette1
-
+            /* Manette 1 */
     // reset the field-centric heading on start press
     manette1.start().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
 
-    //elevateur
-    manette1.a().whileTrue(new Depart(elevateur, pince));
-    manette1.b().whileTrue(new L2(elevateur,pince));
-    manette1.x().whileTrue(new L3(elevateur, pince));
-    manette1.y().whileTrue(new L4(elevateur, pince));
-
     //pince
     manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie));
     manette1.rightBumper().whileTrue(new StationPince(pince, elevateur));
     manette1.leftTrigger().whileTrue(new CoralExpire(pince, bougie));
 
-    //manette2
+     //elevateur
+     manette1.a().whileTrue(new Depart(elevateur, pince));
+     manette1.b().whileTrue(new L2(elevateur,pince));
+     manette1.x().whileTrue(new L3(elevateur, pince));
+     manette1.y().whileTrue(new L4(elevateur, pince));
 
+            /* Manette 2 */
     //pince
     manette2.a().whileTrue(new CorailAspir(pince));
-    manette2.start().whileTrue(new reset(elevateur, pince));
     manette2.b().whileTrue(new Algue_inspire(pince));
-    manette2.start().whileTrue(new reset(elevateur,pince));
+    manette2.y().whileTrue(new BalayeuseHaut(requin));
+    manette2.x().whileTrue(new BalayeuseBas(requin));
+    manette2.rightTrigger().whileTrue(new exspire(requin));
     manette2.leftTrigger().whileTrue(new AlgueExpire(pince, bougie));
 
+     //Pince manuel
+     pince.setDefaultCommand(new RunCommand(()->{
+      pince.pivote(MathUtil.applyDeadband(manette2.getRightY()*manette2.getRightY()*manette2.getRightY(), 0.05));
+    }, pince));
+
+    //Elevateur manuel
+    elevateur.setDefaultCommand(new RunCommand(()->{
+      elevateur.vitesse(MathUtil.applyDeadband(manette2.getLeftY()*manette2.getLeftY()*manette2.getLeftY(), 0.05));
+    }, elevateur));
+
     //limelight
     manette2.leftBumper().whileTrue(new AprilTag3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
     manette2.rightBumper().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY));
+
+    //Reset encodeur
+    manette2.start().whileTrue(new reset(elevateur, pince));
   }
-  
-    // manette2.leftTrigger().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY));
     
     public Command getAutonomousCommand() {
       return new SequentialCommandGroup(Commands.runOnce(()->{

src/main/java/frc/robot/commands/Elevateur/Depart.java

@@ -2,7 +2,7 @@
 // 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;
+package frc.robot.commands.Elevateur;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Elevateur;
@@ -11,12 +11,10 @@ import frc.robot.subsystems.Pince;
 /* 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 Depart extends Command {
   private Elevateur elevateur;
-  private Pince pince;
   /** Creates a new L2. */
   public Depart(Elevateur elevateur, Pince pince) {
-    this.pince = pince;
     this.elevateur = elevateur;
-    addRequirements(elevateur,pince);
+    addRequirements(elevateur);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -41,12 +39,11 @@ public class Depart extends Command {
   @Override
   public void end(boolean interrupted) {
     elevateur.vitesse(0);
-    pince.pivote(0);
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return elevateur.limit2()==true;
+    return elevateur.limit2() == true;
   }
 }

src/main/java/frc/robot/commands/Elevateur/ElevateurManuel.java

@@ -2,7 +2,7 @@
 // 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;
+package frc.robot.commands.Elevateur;
 
 import java.util.function.DoubleSupplier;
 
@@ -28,7 +28,10 @@ public class ElevateurManuel extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (elevateur.limit2())
+    if (elevateur.limit2()){
+
+     
+    }
     elevateur.vitesse(doubleSupplier.getAsDouble()/3.5);
   }
 

src/main/java/frc/robot/commands/Elevateur/L2.java

@@ -2,8 +2,7 @@
 // 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;
-
+package frc.robot.commands.Elevateur;
 import edu.wpi.first.networktables.DoubleSubscriber;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;

src/main/java/frc/robot/commands/Elevateur/L3.java

@@ -2,7 +2,7 @@
 // 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;
+package frc.robot.commands.Elevateur;
 
 import edu.wpi.first.networktables.DoubleSubscriber;
 import edu.wpi.first.networktables.NetworkTable;
@@ -39,8 +39,8 @@ public class L3 extends Command {
   public void execute() {
     double encodeurHaute = encodeur1.get();
     double encodeurBase = encodeur2.get();
-    double encodeurbase = encodeur3.get();
-    double encodeurhaute = encodeur4.get();
+    double encodeurbasp = encodeur3.get();
+    double encodeurhautp = encodeur4.get();
     if(elevateur.position()<=-encodeurHaute && elevateur.position()>=encodeurBase){
       elevateur.vitesse(0);
     }

src/main/java/frc/robot/commands/Elevateur/L4.java

@@ -2,7 +2,7 @@
 // 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;
+package frc.robot.commands.Elevateur;
 
 import edu.wpi.first.networktables.DoubleSubscriber;
 import edu.wpi.first.networktables.DoubleTopic;

src/main/java/frc/robot/commands/Pince/DepartPince.java

@@ -2,7 +2,7 @@
 // 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;
+package frc.robot.commands.Pince;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Pince;

src/main/java/frc/robot/commands/Pince/PinceManuel.java

@@ -2,7 +2,7 @@
 // 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;
+package frc.robot.commands.Pince;
 import java.util.function.DoubleSupplier;
 
 import edu.wpi.first.wpilibj2.command.Command;

src/main/java/frc/robot/commands/grimpeur/GrimperHaut.java

@@ -2,7 +2,7 @@
 // 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;
+package frc.robot.commands.grimpeur;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Bougie;

src/main/java/frc/robot/commands/grimpeur/GrimpeurBas.java

@@ -2,7 +2,7 @@
 // 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;
+package frc.robot.commands.grimpeur;
 
 import edu.wpi.first.networktables.DoubleSubscriber;
 import edu.wpi.first.networktables.NetworkTable;

src/main/java/frc/robot/commands/grimpeur/GrimpeurManuel.java

@@ -2,7 +2,7 @@
 // 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;
+package frc.robot.commands.grimpeur;
 
 import java.util.function.DoubleSupplier;
 

src/main/java/frc/robot/commands/grimpeur/ResetGrimpeur.java

@@ -2,7 +2,7 @@
 // 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;
+package frc.robot.commands.grimpeur;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Grimpeur;

src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java

@@ -0,0 +1,63 @@
+// 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.requin;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Bougie;
+import frc.robot.subsystems.Requin;
+
+/* 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 BalayeuseAlgue extends Command {
+  private Requin requin;
+  private Bougie bougie;
+  /** Creates a new Balayeuse. */
+  public BalayeuseAlgue(Requin requin, Bougie bougie) {
+    this.requin = requin;
+    this.bougie =bougie;
+    addRequirements(requin,bougie);
+    // 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(requin.encodeur()>=500 &&  requin.encodeur()<=510)
+     {
+       requin.rotationer(0);
+       if(requin.amp()> 60){
+       requin.balaye(0);
+       bougie.Vert();
+     }
+      else
+      {
+           requin.balaye(0.5);
+      }
+     }
+     else if(requin.encodeur()>=510){
+     requin.rotationer(0.5);
+     }
+     else{
+      requin.rotationer(-0.5);
+     }
+    
+   }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    requin.rotationer(0);
+    requin.balaye(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/requin/BalayeuseBas.java

@@ -0,0 +1,40 @@
+// 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.requin;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Requin;
+
+/* 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 BalayeuseBas extends Command {
+  private Requin requin;
+  /** Creates a new Balayeuse. */
+  public BalayeuseBas(Requin requin) {
+    this.requin = requin;
+    addRequirements(requin);
+    // 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() {
+  requin.rotationer(-0.5);
+  }
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    requin.rotationer(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/requin/BalayeuseCoral.java

@@ -0,0 +1,67 @@
+// 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.requin;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Bougie;
+import frc.robot.subsystems.Requin;
+
+/* 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 BalayeuseCoral extends Command {
+  private Requin requin;
+  private Bougie bougie;
+  /** Creates a new Balayeuse. */
+  public BalayeuseCoral(Requin requin, Bougie bougie) {
+    this.requin = requin;
+    this.bougie = bougie;
+    addRequirements(requin,bougie);
+    // 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(requin.encodeur()>=100 &&  requin.encodeur()<=110){
+       requin.rotationer(0);
+      if(requin.amp()>60){
+      requin.balaye(0);
+       bougie.Vert();
+       if(requin.enHaut()){
+        requin.rotationer(0);
+       }
+        else{
+          requin.rotationer(0.5);
+        }
+    }
+     else{
+       requin.balaye(0.5);
+     }
+     }
+     else if(requin.encodeur()>=110){
+     requin.rotationer(0.5);
+     }
+     else{
+       requin.rotationer(-0.5);
+     }
+    
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    requin.rotationer(0);
+    requin.balaye(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/requin/BalayeuseHaut.java

@@ -2,22 +2,18 @@
 // 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;
+package frc.robot.commands.requin;
+
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.Pince;
+import frc.robot.subsystems.Requin;
 
 /* 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 PinceManuel2 extends Command {
-  private Pince pince;
-  //private DoubleSupplier doubleSupplier;
-  /** Creates a new PinceManuel. */
-  public PinceManuel2(Pince pince
-   
-  //,DoubleSupplier doubleSupplier
-  ) {
-    this.pince = pince;
-   // this.doubleSupplier = doubleSupplier;
-    addRequirements(pince);
+public class BalayeuseHaut extends Command {
+  private Requin requin;
+  /** Creates a new Balayeuse. */
+  public BalayeuseHaut(Requin requin) {
+    this.requin = requin;
+    addRequirements(requin);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -28,15 +24,19 @@ public class PinceManuel2 extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-
-      pince.pivote(-0.2);
-
+    if(requin.enHaut()==true){
+      requin.rotationer(0);
+      requin.reset();
+    }
+    else{
+      requin.rotationer(-0.5);
+    }
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    pince.pivote(0);
+    requin.rotationer(0);
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/commands/requin/ExpireAlgue.java

@@ -0,0 +1,41 @@
+// 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.requin;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Requin;
+
+/* 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 ExpireAlgue extends Command {
+  private Requin requin;
+  /** Creates a new ExpireAlgue. */
+  public ExpireAlgue(Requin requin) {
+    this.requin = requin;
+    addRequirements(requin);
+    // 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() {
+    requin.balaye(0.5);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    requin.balaye(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/requin/L1Requin.java

@@ -0,0 +1,65 @@
+// 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.requin;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Bougie;
+import frc.robot.subsystems.Requin;
+
+/* 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 L1Requin extends Command {
+  private Requin requin;
+  private Bougie bougie;
+  /** Creates a new Balayeuse. */
+  public L1Requin(Requin requin,Bougie bougie) {
+    this.requin = requin;
+    this.bougie = bougie;
+    addRequirements(requin,bougie);
+    // 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(requin.encodeur()>=800 &&  requin.encodeur()<=810){
+      requin.rotationer(0);
+      if(requin.amp()>8){
+      requin.balaye(-0.5);
+    }
+        else{
+          requin.balaye(0); 
+          bougie.Rouge();
+      }
+    }
+    else if(requin.encodeur()>=810){
+    requin.rotationer(0.5);
+    }
+    else{
+      requin.rotationer(-0.5);
+    }
+    
+    
+      
+    
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    requin.rotationer(0);
+    requin.balaye(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/requin/aspire.java

@@ -2,19 +2,19 @@
 // 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.Pince;
+package frc.robot.commands.requin;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.Pince;
+import frc.robot.subsystems.Requin;
 
 /* 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 Algue1Test extends Command {
-  private Pince pince;
-  /** Creates a new AlgueTest. */
-  public Algue1Test(Pince pince) {
-    this.pince = pince;
-    addRequirements(pince);
+public class aspire extends Command {
+  /** Creates a new aspire. */
+  private Requin requin;
+  public aspire(Requin requin) {
     // Use addRequirements() here to declare subsystem dependencies.
+    this.requin = requin;
+    addRequirements(requin);
   }
 
   // Called when the command is initially scheduled.
@@ -24,13 +24,13 @@ public class Algue1Test extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    pince.algue1Test(0.2);
+    requin.balaye(0.3);
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    pince.algue1Test(0);
+    requin.balaye(0);
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/commands/requin/exspire.java

@@ -2,19 +2,19 @@
 // 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.Pince;
+package frc.robot.commands.requin;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.Pince;
+import frc.robot.subsystems.Requin;
 
 /* 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 Algue2Test extends Command {
-  private Pince pince;
-  /** Creates a new AlgueTest. */
-  public Algue2Test(Pince pince) {
-    this.pince = pince;
-    addRequirements(pince);
+public class exspire extends Command {
+  /** Creates a new aspire. */
+  private Requin requin;
+  public exspire(Requin requin) {
     // Use addRequirements() here to declare subsystem dependencies.
+    this.requin = requin;
+    addRequirements(requin);
   }
 
   // Called when the command is initially scheduled.
@@ -24,13 +24,13 @@ public class Algue2Test extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    pince.algue2Test(0.2);
+    requin.balaye(-0.3);
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    pince.algue2Test(0);
+    requin.balaye(0);
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/commands/requin/requin_manuel.java

@@ -0,0 +1,49 @@
+// 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.requin;
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Requin;
+
+/* 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 requin_manuel extends Command {
+  /** Creates a new requin_manuel. */
+  private Requin requin;
+  private DoubleSupplier x;
+  public requin_manuel(Requin requin) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.requin = requin;
+    addRequirements(requin);
+  }
+
+  // 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(requin.enHaut()){
+    requin.rotationer(0);  
+    }
+    else{
+      requin.rotationer(x.getAsDouble());
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    requin.rotationer(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/subsystems/Bougie.java

@@ -43,8 +43,12 @@ public class Bougie extends SubsystemBase {
    candle.setLEDs(255, 215, 0,0,48,8);
    candle.setLEDs(255, 215, 0,0,64,8);
    }
-  public void RainBow(){candle.animate(rainbowAnim);}
-  public void RainBowStop(){candle.animate(null);}
+  public void RainBow(){
+    candle.animate(rainbowAnim);
+  }
+  public void RainBowStop(){
+    candle.animate(null);
+  }
   @Override
   public void periodic() {
     // This method will be called once per scheduler run

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -22,7 +22,6 @@ import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.TunerConstants.TunerConstants.TunerSwerveDrivetrain;
 
 /**
@@ -44,71 +43,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
 
     private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds();
 
-    /* Swerve requests to apply during SysId characterization */
-    private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation();
-    private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
-    private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
-
     /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
 
-    private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine(
-        new SysIdRoutine.Config(
-            null,        // Use default ramp rate (1 V/s)
-            Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout
-            null,        // Use default timeout (10 s)
-            // Log state with SignalLogger class
-            state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())
-        ),
-        new SysIdRoutine.Mechanism(
-            output -> setControl(m_translationCharacterization.withVolts(output)),
-            null,
-            this
-        )
-    );
-
-    /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */
-    private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine(
-        new SysIdRoutine.Config(
-            null,        // Use default ramp rate (1 V/s)
-            Volts.of(7), // Use dynamic voltage of 7 V
-            null,        // Use default timeout (10 s)
-            // Log state with SignalLogger class
-            state -> SignalLogger.writeString("SysIdSteer_State", state.toString())
-        ),
-        new SysIdRoutine.Mechanism(
-            volts -> setControl(m_steerCharacterization.withVolts(volts)),
-            null,
-            this
-        )
-    );
-
-    /*
-     * SysId routine for characterizing rotation.
-     * This is used to find PID gains for the FieldCentricFacingAngle HeadingController.
-     * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId.
-     */
-    private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine(
-        new SysIdRoutine.Config(
-            /* This is in radians per second², but SysId only supports "volts per second" */
-            Volts.of(Math.PI / 6).per(Second),
-            /* This is in radians per second, but SysId only supports "volts" */
-            Volts.of(Math.PI),
-            null, // Use default timeout (10 s)
-            // Log state with SignalLogger class
-            state -> SignalLogger.writeString("SysIdRotation_State", state.toString())
-        ),
-        new SysIdRoutine.Mechanism(
-            output -> {
-                /* output is actually radians per second, but SysId only supports "volts" */
-                setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts)));
-                /* also log the requested output for SysId */
-                SignalLogger.writeDouble("Rotational_Rate", output.in(Volts));
-            },
-            null,
-            this
-        )
-    );
-    
     private void configureAutoBuilder() {
         try {
             var config = RobotConfig.fromGUISettings();
@@ -140,9 +76,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
             return 0;
          });  
         }
- 
-    /* The SysId routine to test */
-    private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineSteer;
 
     /**
      * Constructs a CTRE SwerveDrivetrain using the specified constants.
@@ -234,28 +167,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     public Command applyRequest(Supplier<SwerveRequest> requestSupplier) {
         return run(() -> this.setControl(requestSupplier.get()));
     }
-
-    /**
-     * Runs the SysId Quasistatic test in the given direction for the routine
-     * specified by {@link #m_sysIdRoutineToApply}.
-     *
-     * @param direction Direction of the SysId Quasistatic test
-     * @return Command to run
-     */
-    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
-        return m_sysIdRoutineToApply.quasistatic(direction);
-    }
-
-    /**
-     * Runs the SysId Dynamic test in the given direction for the routine
-     * specified by {@link #m_sysIdRoutineToApply}.
-     *
-     * @param direction Direction of the SysId Dynamic test
-     * @return Command to run
-     */
-    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
-        return m_sysIdRoutineToApply.dynamic(direction);
-    }
     
     @Override
     public void periodic() {

src/main/java/frc/robot/subsystems/Elevateur.java

@@ -3,8 +3,6 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package frc.robot.subsystems;
-
-import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -25,7 +23,17 @@ public class Elevateur extends SubsystemBase {
     return monte.getEncoder().getPosition();
   }
   public void vitesse(double vitesse){
-    monte.set(vitesse);
+    if (limit2()) {
+      if (vitesse > 0) {
+        monte.set(0);
+      }
+      else{
+        monte.set(vitesse);
+      }
+    }
+    else{
+      monte.set(vitesse);
+    }
   }
   public boolean limit2(){
     return limit2.get();

src/main/java/frc/robot/subsystems/Pince.java

@@ -6,8 +6,6 @@ package frc.robot.subsystems;
 
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.spark.SparkMax;
-
-import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -27,11 +25,21 @@ public class Pince extends SubsystemBase {
   final DigitalInput limit6 = new DigitalInput(9);
   
  
-  public void aspirecoral(double vitesse){
-    coral.set(vitesse);
-  }
+public void aspirecoral(double vitesse){
+  coral.set(vitesse);
+}
 public void pivote(double vitesse){
-  pivoti.set(vitesse);
+  if (position()) {
+    if (vitesse > 0) {
+      pivoti.set(0);
+    }
+    else{
+      pivoti.set(vitesse);
+    }
+  }
+  else{
+    pivoti.set(vitesse);
+  }
 }
 public void aspirealgue(double vitesse){
   algue2.set(-vitesse);
@@ -52,12 +60,7 @@ public double emperagecoral(){
 public double emperagealgue(){
   return algue1.getOutputCurrent();
 }
-public void algue1Test(double vitesse){
-  algue1.set(vitesse);
-}
-public void algue2Test(double vitesse){
-  algue2.set(vitesse);
-}
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run

src/main/java/frc/robot/subsystems/Requin.java

@@ -0,0 +1,48 @@
+// 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.subsystems;
+
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Requin extends SubsystemBase {
+  /** Creates a new Requin. */
+  ShuffleboardTab teb = Shuffleboard.getTab("teb");
+  public Requin() {
+    teb.addBoolean("limit requin", this::enHaut);
+  }
+   
+  final SparkFlex balaye = new SparkFlex(15, MotorType.kBrushless);
+  final SparkMax rotatione = new SparkMax(17, MotorType.kBrushless);
+  final DigitalInput limit3 = new DigitalInput(1);
+    
+  public void balaye(double vitesse){
+    balaye.set(vitesse);
+  }
+  public void rotationer(double vitesse){
+  rotatione.set(vitesse);
+  }
+  public boolean enHaut(){
+  return limit3.get();
+  } 
+  public double encodeur(){
+  return rotatione.getEncoder().getPosition();
+  }
+  public void reset(){
+    rotatione.getEncoder().setPosition(0);
+  }
+  public double amp(){
+    return balaye.getOutputCurrent();
+  }
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}