diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index daf473c..239c7f8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,10 +12,11 @@ import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; // Manette - +import edu.wpi.first.wpilibj2.command.WaitCommand; // Manettes import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -29,6 +30,8 @@ import frc.robot.command.GuiderHaut; import frc.robot.command.Lancer; import frc.robot.command.LancerNote; import frc.robot.command.Lancerampli; +import frc.robot.command.Limelight_tracker; +import frc.robot.command.Pistongrimpeur; // Subsystems import frc.robot.subsystem.Accumulateur; import frc.robot.subsystem.Balayeuse; @@ -55,6 +58,7 @@ public class RobotContainer { CommandJoystick joystick = new CommandJoystick(0); CommandXboxController manette = new CommandXboxController(1); //command + Limelight_tracker limelight_tracker = new Limelight_tracker(limelight,drive); Balayer balayer = new Balayer(balayeuse, accumulateur); GuiderHaut guiderHaut = new GuiderHaut(guideur); GuiderBas guiderBas = new GuiderBas(guideur); @@ -64,6 +68,7 @@ public class RobotContainer { GrimpeurDroit grimpeurDroit = new GrimpeurDroit(grimpeur, manette::getLeftX); GrimpeurGauche grimpeurGauche = new GrimpeurGauche(grimpeur, manette::getLeftY); AllumeLED allumeLED = new AllumeLED(LED); + Pistongrimpeur pistongrimpeur = new Pistongrimpeur(grimpeur, LED); public RobotContainer() { NamedCommands.registerCommand("balayer",new Balayer(balayeuse, accumulateur)); NamedCommands.registerCommand("lancer", new LancerNote(lanceur, accumulateur)); @@ -74,6 +79,8 @@ public class RobotContainer { manette.b().whileTrue(guiderHaut); joystick.button(3).toggleOnTrue(balayer); joystick.button(1).whileTrue(lancernote); + joystick.button(4).whileTrue(new ParallelCommandGroup(lancer,limelight_tracker)); + joystick.button(2).whileTrue(new ParallelCommandGroup(lancerampli,limelight_tracker,guiderHaut)); configureBindings(); drive.setDefaultCommand(new RunCommand(()->{ @@ -83,6 +90,7 @@ public class RobotContainer { grimpeur.droit(manette.getLeftX());} ,grimpeur)); LED.setDefaultCommand(allumeLED); + } private void configureBindings() { diff --git a/src/main/java/frc/robot/command/Pistongrimpeur.java b/src/main/java/frc/robot/command/Pistongrimpeur.java new file mode 100644 index 0000000..56df958 --- /dev/null +++ b/src/main/java/frc/robot/command/Pistongrimpeur.java @@ -0,0 +1,49 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.command; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystem.Grimpeur; +import frc.robot.subsystem.LED; + +public class Pistongrimpeur extends Command { + private LED LED; + private Grimpeur grimpeur; + /** Creates a new Pistongrimpeur. */ + public Pistongrimpeur(Grimpeur grimpeur,LED LED) { + this.grimpeur = grimpeur; + this.LED = LED; + addRequirements(grimpeur,LED); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + grimpeur.pistonouvre(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + if(grimpeur.pistonouvre()){ + LED.couleur(0, 0, 255); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + LED.couleur(0, 0, 0); + grimpeur.pistonferme(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystem/Grimpeur.java b/src/main/java/frc/robot/subsystem/Grimpeur.java index e2cbc93..3ba9d85 100644 --- a/src/main/java/frc/robot/subsystem/Grimpeur.java +++ b/src/main/java/frc/robot/subsystem/Grimpeur.java @@ -76,13 +76,15 @@ public AHRS gyroscope = new AHRS(); public double getpitch(){ return gyroscope.getPitch(); } - public void pistonouvre(){ + public boolean pistonouvre(){ pistondroite.set(Value.kForward); pistondgauche.set(Value.kForward); + return pistondroite.isFwdSolenoidDisabled(); } public void pistonferme(){ pistondroite.set(Value.kReverse); pistondgauche.set(Value.kReverse); + } @Override public void periodic() {