moyenne d'amps

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

@@ -21,6 +21,7 @@ public class Lancer extends Command {
   private Timer timer;
   private Balayeuse balayeuse;
   private Led led;
+  private double temp;
   /** Creates a new Lancer. */
   public Lancer(Lanceur lanceur, LimeLight3 limeLight3, Balayeuse balayeuse,Led led) {
     this.lanceur = lanceur;
@@ -29,6 +30,7 @@ public class Lancer extends Command {
     this.timer = new Timer();
     this.limeLight3G = new Limelight3G();
     addRequirements(lanceur);
+    this.temp = 0;
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -37,39 +39,38 @@ public class Lancer extends Command {
   public void initialize() {
     pidController = new PIDController(0, 0,0, 0);
     timer.reset();
+    timer.start();
+    temp = lanceur.Amp();    
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    timer.start();
-    if(lanceur.Amp() > 40){ 
+    int nbFois = 0;
+
+    double moyenneAmp = 0;
+    if(timer.get() < 3){
+      nbFois++;
+      moyenneAmp += balayeuse.Amp() / nbFois;
+    }
+    else{
+      nbFois++;
+      moyenneAmp -= temp;
+      moyenneAmp += balayeuse.Amp() / nbFois;
+      temp = balayeuse.Amp();
+    }
+    if(moyenneAmp > 30 && nbFois > 10){
       timer.reset();
-      double vitesse = (100-limeLight3G.getTA())/100;
+      balayeuse.Balayer(0.5);
+    }
+    else{
+      double vitesse = (100-limeLight3G.getTA())/lanceur.Vitesse();
       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())/lanceur.Vitesse();
-        double output = pidController.calculate(lanceur.Vitesse(),vitesse);
-        lanceur.Lancer(output);
-        if(lanceur.Vitesse() >= vitesse){
-          lanceur.Demeler(0.5);
-        }
-      }
-      led.Vert2();
-    }
 
   }