led fini

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

@@ -5,8 +5,11 @@
 package frc.robot.commands;
 
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Balayeuse;
 import frc.robot.subsystems.Lanceur;
+import frc.robot.subsystems.Led;
 import frc.robot.subsystems.LimeLight3;
 import frc.robot.subsystems.Limelight3G;
 
@@ -15,10 +18,15 @@ public class Lancer extends Command {
   private Lanceur lanceur;
   private PIDController pidController;
   private Limelight3G limeLight3G;
-  private double output;
+  private Timer timer;
+  private Balayeuse balayeuse;
+  private Led led;
   /** Creates a new Lancer. */
-  public Lancer(Lanceur lanceur, LimeLight3 limeLight3) {
+  public Lancer(Lanceur lanceur, LimeLight3 limeLight3, Balayeuse balayeuse,Led led) {
     this.lanceur = lanceur;
+    this.balayeuse = balayeuse;
+    this.led = led;
+    this.timer = new Timer();
     this.limeLight3G = new Limelight3G();
     addRequirements(lanceur);
     // Use addRequirements() here to declare subsystem dependencies.
@@ -28,17 +36,41 @@ public class Lancer extends Command {
   @Override
   public void initialize() {
     pidController = new PIDController(0, 0,0, 0);
+    timer.reset();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double vitesse = (100-limeLight3G.getTA())/100;
-    double output = pidController.calculate(lanceur.Vitesse(),vitesse);
-    lanceur.Lancer(output);
-    if(lanceur.Vitesse() >= vitesse){
-      lanceur.Demeler(0.5);
+    timer.start();
+    if(lanceur.Amp() > 40){ 
+      timer.reset();
+      double vitesse = (100-limeLight3G.getTA())/100;
+      double output = pidController.calculate(lanceur.Vitesse(),vitesse);
+      lanceur.Lancer(output);
+      if(lanceur.Vitesse() >= vitesse){
+        lanceur.Demeler(0.5);
+      }
     }
+    else if(lanceur.Amp() < 40){
+      lanceur.Lancer(0);
+      lanceur.Demeler(0);
+      if(!balayeuse.GetLimiSwtich()){
+        balayeuse.Pivoter(0.2);
+      }
+      else{
+        balayeuse.Reset();
+        balayeuse.Pivoter(0);
+        double vitesse = (100-limeLight3G.getTA())/100;
+        double output = pidController.calculate(lanceur.Vitesse(),vitesse);
+        lanceur.Lancer(output);
+        if(lanceur.Vitesse() >= vitesse){
+          lanceur.Demeler(0.5);
+        }
+      }
+      led.Vert2();
+    }
+
   }
 
   // Called once the command ends or is interrupted.
@@ -46,6 +78,7 @@ public class Lancer extends Command {
   public void end(boolean interrupted) {
     lanceur.Demeler(0);
     lanceur.Lancer(0);
+    balayeuse.Pivoter(0);
   }
 
   // Returns true when the command should end.