diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 12bf559..3fb4b54 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,8 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; // Commands import frc.robot.command.Balayer; +import frc.robot.command.GrimpeurDroit; +import frc.robot.command.GrimpeurGauche; import frc.robot.command.GuiderBas; import frc.robot.command.GuiderHaut; import frc.robot.command.Lancer; @@ -52,7 +54,8 @@ public class RobotContainer { Lancerampli lancerampli = new Lancerampli(lanceur); CommandJoystick joystick = new CommandJoystick(0); CommandXboxController manette = new CommandXboxController(1); - + GrimpeurDroit grimpeurDroit = new GrimpeurDroit(grimpeur, manette::getLeftX); + GrimpeurGauche grimpeurGauche = new GrimpeurGauche(grimpeur, manette::getLeftY); public RobotContainer() { NamedCommands.registerCommand("balayer",new Balayer(balayeuse, accumulateur)); NamedCommands.registerCommand("lancer", new LancerNote(lanceur, accumulateur)); @@ -69,8 +72,8 @@ public class RobotContainer { } private void configureBindings() { - - joystick.button(3).toggleOnTrue(balayer); + + joystick.button(3).toggleOnTrue(balayer); } diff --git a/src/main/java/frc/robot/command/GrimpeurHautDroit.java b/src/main/java/frc/robot/command/GrimpeurDroit.java similarity index 67% rename from src/main/java/frc/robot/command/GrimpeurHautDroit.java rename to src/main/java/frc/robot/command/GrimpeurDroit.java index 324501f..0aa2c46 100644 --- a/src/main/java/frc/robot/command/GrimpeurHautDroit.java +++ b/src/main/java/frc/robot/command/GrimpeurDroit.java @@ -4,13 +4,17 @@ package frc.robot.command; +import java.util.function.DoubleSupplier; + import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystem.Grimpeur; -public class GrimpeurHautDroit extends Command { +public class GrimpeurDroit extends Command { + private DoubleSupplier doubleSupplier; private Grimpeur grimpeur; /** Creates a new GrimpeurHaut. */ - public GrimpeurHautDroit(Grimpeur grimpeur) { + public GrimpeurDroit(Grimpeur grimpeur, DoubleSupplier doubleSupplier) { + this.doubleSupplier = doubleSupplier; this.grimpeur = grimpeur; addRequirements(grimpeur); // Use addRequirements() here to declare subsystem dependencies. @@ -20,26 +24,32 @@ public class GrimpeurHautDroit extends Command { @Override public void initialize() { grimpeur.resetencodeurd(); - grimpeur.resetencodeurg(); + grimpeur.pistonferme(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - - if(grimpeur.encoderd()>261 ){ + grimpeur.droit(doubleSupplier.getAsDouble()); + if(grimpeur.encoderd()>261){ grimpeur.droit(0); + } else if(grimpeur.getpitch()<-15){ - grimpeur.droit(-0.6); + grimpeur.droit(-doubleSupplier.getAsDouble()); } else if(grimpeur.getpitch()>15){ - grimpeur.droit(0.6); + grimpeur.droit(doubleSupplier.getAsDouble()); } else{ grimpeur.droit(0); } + if(grimpeur.encoderd()>0){ + grimpeur.resetencodeurd(); + grimpeur.droit(0); + + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/command/GrimpeurHautGauche.java b/src/main/java/frc/robot/command/GrimpeurGauche.java similarity index 69% rename from src/main/java/frc/robot/command/GrimpeurHautGauche.java rename to src/main/java/frc/robot/command/GrimpeurGauche.java index 2bd6593..11a5685 100644 --- a/src/main/java/frc/robot/command/GrimpeurHautGauche.java +++ b/src/main/java/frc/robot/command/GrimpeurGauche.java @@ -4,13 +4,17 @@ package frc.robot.command; +import java.util.function.DoubleSupplier; + import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystem.Grimpeur; -public class GrimpeurHautGauche extends Command { +public class GrimpeurGauche extends Command { + private DoubleSupplier doubleSupplier; private Grimpeur grimpeur; /** Creates a new GrimpeurHautGauche. */ - public GrimpeurHautGauche(Grimpeur grimpeur) { + public GrimpeurGauche(Grimpeur grimpeur,DoubleSupplier doubleSupplier) { + this.doubleSupplier = doubleSupplier; this.grimpeur = grimpeur; addRequirements(grimpeur); // Use addRequirements() here to declare subsystem dependencies. @@ -26,19 +30,25 @@ public class GrimpeurHautGauche extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(grimpeur.encoderg()>261 ){ + grimpeur.gauche(doubleSupplier.getAsDouble()); + if(grimpeur.encoderg()>261){ grimpeur.gauche(0); } else if(grimpeur.getpitch()<-15){ - grimpeur.gauche(0.6); + grimpeur.gauche(doubleSupplier.getAsDouble()); } else if(grimpeur.getpitch()>15){ - grimpeur.gauche(-0.6); + grimpeur.gauche(-doubleSupplier.getAsDouble()); } else{ grimpeur.gauche(0); } + if(grimpeur.encoderd()>0){ + grimpeur.resetencodeurg(); + grimpeur.gauche(0); + + } } // Called once the command ends or is interrupted.