diff --git a/src/main/java/frc/robot/commands/Aspirer.java b/src/main/java/frc/robot/commands/Aspirer.java index 88ba264..69296de 100644 --- a/src/main/java/frc/robot/commands/Aspirer.java +++ b/src/main/java/frc/robot/commands/Aspirer.java @@ -48,7 +48,7 @@ public class Aspirer extends Command { moyenneAmp += balayeuse.Amp() / nbFois; temp = balayeuse.Amp(); } - if(moyenneAmp < 40){ + if(moyenneAmp < balayeuse.AmpMax()){ timer.reset(); balayeuse.Balayer(0.5); } diff --git a/src/main/java/frc/robot/commands/Lancer.java b/src/main/java/frc/robot/commands/Lancer.java index d3816b0..cc19ef5 100644 --- a/src/main/java/frc/robot/commands/Lancer.java +++ b/src/main/java/frc/robot/commands/Lancer.java @@ -59,7 +59,7 @@ public class Lancer extends Command { moyenneAmp += balayeuse.Amp() / nbFois; temp = balayeuse.Amp(); } - if(moyenneAmp > 30 && nbFois > 10){ + if(moyenneAmp > lanceur.AmpBas() && nbFois > 10){ timer.reset(); balayeuse.Balayer(0.5); led.Jaune2(); diff --git a/src/main/java/frc/robot/subsystems/Balayeuse.java b/src/main/java/frc/robot/subsystems/Balayeuse.java index d0ffb4e..85eb747 100644 --- a/src/main/java/frc/robot/subsystems/Balayeuse.java +++ b/src/main/java/frc/robot/subsystems/Balayeuse.java @@ -23,6 +23,8 @@ public class Balayeuse extends SubsystemBase { DigitalInput limit = new DigitalInput(0); private GenericEntry EncodeurBalayeuse = teb.add("Position bas balayeuse", 10).getEntry(); + private GenericEntry AmpBaleyeuse = + teb.add("Ampérage Baleyeuse", 40).getEntry(); public void Balayer(double vitesse){ Balaye1.set(vitesse); Balaye2.set(vitesse); @@ -42,6 +44,9 @@ public class Balayeuse extends SubsystemBase { public double Amp(){ return Balaye2.getOutputCurrent(); } + public double AmpMax(){ + return AmpBaleyeuse.getDouble(40); + } public void Temps(){ Timer timer = new Timer(); timer.start(); diff --git a/src/main/java/frc/robot/subsystems/Lanceur.java b/src/main/java/frc/robot/subsystems/Lanceur.java index cd50440..eac8add 100644 --- a/src/main/java/frc/robot/subsystems/Lanceur.java +++ b/src/main/java/frc/robot/subsystems/Lanceur.java @@ -19,6 +19,8 @@ public class Lanceur extends SubsystemBase { SparkFlex Demeleur = new SparkFlex(4, MotorType.kBrushless); GenericEntry vitesse = teb.add("vitesse lanceur",100).getEntry(); + GenericEntry AmpLanceur = + teb.add("ampérage lanceur",30).getEntry(); public void Lancer(double vitesse){ moteur1.set(vitesse); moteur2.set(-vitesse); @@ -32,6 +34,9 @@ public class Lanceur extends SubsystemBase { public double Amp(){ return moteur1.getOutputCurrent(); } + public double AmpBas(){ + return AmpLanceur.getDouble(30); + } public double vitesseDemander(){ return vitesse.getDouble(100); }