From a142290bd50632b16c9f87e649e70c0a31a11c3e Mon Sep 17 00:00:00 2001 From: samuel desharnais Date: Tue, 10 Mar 2026 20:39:18 -0400 Subject: [PATCH] balayeuse fait --- src/main/java/frc/robot/RobotContainer.java | 1 + src/main/java/frc/robot/commands/Aspirer.java | 2 +- .../frc/robot/commands/DescendreGrimpeur.java | 2 +- src/main/java/frc/robot/commands/Lancer.java | 37 ++++++++++------- .../frc/robot/commands/LancerBaseVitesse.java | 40 +++++++++---------- .../frc/robot/commands/MonterGrimpeur.java | 2 +- .../java/frc/robot/subsystems/Grimpeur.java | 4 +- .../java/frc/robot/subsystems/Lanceur.java | 8 ++-- 8 files changed, 52 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cd1656d..a237292 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -93,6 +93,7 @@ public class RobotContainer { .withRotationalRate(MathUtil.applyDeadband(-manette.getRightX()*Math.abs(-manette.getRightX())*MaxAngularRate, 0.05)) ) ); + manette.povUp().whileTrue(new LancerAuto(lanceur)); manette.a().whileTrue(new Lancer(lanceur,limeLight3G,balayeuse,led)); manette.y().whileTrue(new ModeOposer(lanceur, balayeuse)); manette.x().whileTrue(new LancerBaseVitesse(lanceur, limeLight3, balayeuse, led)); diff --git a/src/main/java/frc/robot/commands/Aspirer.java b/src/main/java/frc/robot/commands/Aspirer.java index 69296de..ffe751d 100644 --- a/src/main/java/frc/robot/commands/Aspirer.java +++ b/src/main/java/frc/robot/commands/Aspirer.java @@ -50,7 +50,7 @@ public class Aspirer extends Command { } if(moyenneAmp < balayeuse.AmpMax()){ timer.reset(); - balayeuse.Balayer(0.5); + balayeuse.Balayer(-0.5); } else{ balayeuse.Balayer(0); diff --git a/src/main/java/frc/robot/commands/DescendreGrimpeur.java b/src/main/java/frc/robot/commands/DescendreGrimpeur.java index ed4ea9e..4078f0b 100644 --- a/src/main/java/frc/robot/commands/DescendreGrimpeur.java +++ b/src/main/java/frc/robot/commands/DescendreGrimpeur.java @@ -24,7 +24,7 @@ public class DescendreGrimpeur extends Command { @Override public void execute() { if(!grimpeur.Limit()){ - grimpeur.Grimper(-0.2); + grimpeur.Grimper(-0.3); } else{ grimpeur.Reset(); diff --git a/src/main/java/frc/robot/commands/Lancer.java b/src/main/java/frc/robot/commands/Lancer.java index 5a7674b..c4a3db4 100644 --- a/src/main/java/frc/robot/commands/Lancer.java +++ b/src/main/java/frc/robot/commands/Lancer.java @@ -10,7 +10,6 @@ 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; /* 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 */ @@ -37,7 +36,7 @@ public class Lancer extends Command { // Called when the command is initially scheduled. @Override public void initialize() { - pidController = new PIDController(0, 0,0, 0); + pidController = new PIDController(1, 0,0, 0); timer.reset(); timer.start(); temp = lanceur.Amp(); @@ -46,10 +45,14 @@ public class Lancer extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + double botx = 0; + double boty = 0; double[] BotPose = new double[6]; - BotPose = limeLight3G.getBotPoseBlue(); - double botx = BotPose[0]; - double boty = BotPose[1]; + if(limeLight3G.getV()){ + BotPose = limeLight3G.getBotPoseBlue(); + botx = BotPose[0]; + boty = BotPose[1]; + } int nbFois = 0; double moyenneAmp = 0; @@ -63,21 +66,25 @@ public class Lancer extends Command { moyenneAmp += balayeuse.Amp() / nbFois; temp = balayeuse.Amp(); } - if(moyenneAmp > lanceur.AmpBas() && nbFois > 10){ + // if(moyenneAmp > lanceur.AmpBas() && nbFois > 10){ timer.reset(); - balayeuse.Balayer(0.5); - led.Jaune2(); - } - else{ - //pythagore | - // \/ - double vitesse = Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)); - double output = pidController.calculate(lanceur.Vitesse(),vitesse); + balayeuse.Balayer(-0.5); + // led.Jaune2(); + // } + // else{ + + double vitesse = 0.5; + if(limeLight3G.getV()){ + //pythagore | + // \/ + vitesse = Math.sqrt(Math.pow(Math.abs(4.625594-botx), 2) + Math.pow(Math.abs(4.034536-boty), 2)); + } + double output = vitesse; /*pidController.calculate(lanceur.Vitesse(),vitesse);*/ lanceur.Lancer(output); if(lanceur.Vitesse() >= vitesse){ lanceur.Demeler(0.5); } - } + // } } diff --git a/src/main/java/frc/robot/commands/LancerBaseVitesse.java b/src/main/java/frc/robot/commands/LancerBaseVitesse.java index 9453127..44e88e0 100644 --- a/src/main/java/frc/robot/commands/LancerBaseVitesse.java +++ b/src/main/java/frc/robot/commands/LancerBaseVitesse.java @@ -37,7 +37,7 @@ public class LancerBaseVitesse extends Command { // Called when the command is initially scheduled. @Override public void initialize() { - pidController = new PIDController(0, 0,0, 0); + pidController = new PIDController(1, 0,0, 0); timer.reset(); timer.start(); temp = lanceur.Amp(); @@ -46,32 +46,32 @@ public class LancerBaseVitesse extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - int nbFois = 0; + // 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(); - balayeuse.Balayer(0.5); - led.Jaune2(); - } - else{ + // 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(); + // balayeuse.Balayer(0.5); + // led.Jaune2(); + // } + // else{ double vitesse = 0.4; double output = pidController.calculate(lanceur.Vitesse(),vitesse); lanceur.Lancer(output); if(lanceur.Vitesse() >= vitesse){ lanceur.Demeler(0.5); } - } + // } } diff --git a/src/main/java/frc/robot/commands/MonterGrimpeur.java b/src/main/java/frc/robot/commands/MonterGrimpeur.java index 995ac37..fd9b8d6 100644 --- a/src/main/java/frc/robot/commands/MonterGrimpeur.java +++ b/src/main/java/frc/robot/commands/MonterGrimpeur.java @@ -25,7 +25,7 @@ public class MonterGrimpeur extends Command { public void execute() { if(Math.abs(grimpeur.Position()) < grimpeur.PositionFinal()){ - grimpeur.Grimper(0.2); + grimpeur.Grimper(0.3); System.out.println("monte"); } else { diff --git a/src/main/java/frc/robot/subsystems/Grimpeur.java b/src/main/java/frc/robot/subsystems/Grimpeur.java index 7826ae8..da425cf 100644 --- a/src/main/java/frc/robot/subsystems/Grimpeur.java +++ b/src/main/java/frc/robot/subsystems/Grimpeur.java @@ -21,7 +21,7 @@ public class Grimpeur extends SubsystemBase { SparkMaxConfig slaveConfig = new SparkMaxConfig(); DigitalInput limit = new DigitalInput(0); private GenericEntry EncodeurGrimpeur = - teb.add("Position haut grimpeur", 10).getEntry(); + teb.add("Position haut grimpeur", 105).getEntry(); public void Grimper(double vitesse){ grimpeur1.set(vitesse); grimpeur2.set(vitesse); @@ -36,7 +36,7 @@ public class Grimpeur extends SubsystemBase { return limit.get(); } public double PositionFinal(){ - return EncodeurGrimpeur.getDouble(10); + return EncodeurGrimpeur.getDouble(105); } /** Creates a new Grimpeur. */ public Grimpeur() { diff --git a/src/main/java/frc/robot/subsystems/Lanceur.java b/src/main/java/frc/robot/subsystems/Lanceur.java index 7931a6a..3aee063 100644 --- a/src/main/java/frc/robot/subsystems/Lanceur.java +++ b/src/main/java/frc/robot/subsystems/Lanceur.java @@ -18,12 +18,12 @@ public class Lanceur extends SubsystemBase { SparkFlex moteur2 = new SparkFlex(18, MotorType.kBrushless); SparkFlex Demeleur = new SparkFlex(19, MotorType.kBrushless); GenericEntry vitesse = - teb.add("vitesse lanceur",100).getEntry(); + teb.add("vitesse lanceur",0.5).getEntry(); GenericEntry AmpLanceur = teb.add("ampérage lanceur",30).getEntry(); public void Lancer(double vitesse){ - moteur1.set(vitesse); - moteur2.set(-vitesse); + // moteur1.set(-vitesse); + moteur2.set(vitesse); } public void Demeler(double vitesse){ Demeleur.set(vitesse); @@ -38,7 +38,7 @@ public class Lanceur extends SubsystemBase { return AmpLanceur.getDouble(30); } public double vitesseDemander(){ - return vitesse.getDouble(100); + return vitesse.getDouble(0.5); } /** Creates a new Lanceur. */ public Lanceur() {