led fini

src/main/java/frc/robot/commands/Aspirer.java

@@ -4,33 +4,56 @@
 
 package frc.robot.commands;
 
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Balayeuse;
+import frc.robot.subsystems.Led;
 
 /* 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 Aspirer extends Command {
   private Balayeuse balayeuse;
+  private Timer timer;
+  private Led led;
   /** Creates a new Aspirer. */
-  public Aspirer(Balayeuse balayeuse) {
+  public Aspirer(Balayeuse balayeuse, Led led) {
     this.balayeuse = balayeuse;
-    addRequirements(balayeuse); 
+    this.led = led;
+    this.timer = new Timer();
+    addRequirements(balayeuse, led); 
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    timer.reset();
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    balayeuse.Balayer(0.5);
+    timer.start();
+    if(balayeuse.Amp() < 40){
+      timer.reset();
+      balayeuse.Balayer(0.5);
+    }
+    else if(balayeuse.Amp() > 40){
+      if(timer.get() > 2){
+        balayeuse.Balayer(0);
+        led.Jaune2();
+      }
+      else{
+        balayeuse.Balayer(0.5);
+      }
+    }
+
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
     balayeuse.Balayer(0);
+    timer.stop();
   }
 
   // Returns true when the command should end.