diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4ab327e..f9aeadb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,7 @@ import frc.robot.commands.Aspirer; import frc.robot.commands.DescendreBalyeuse; import frc.robot.commands.DescendreGrimpeur; import frc.robot.commands.Lancer; +import frc.robot.commands.LancerBaseVitesse; import frc.robot.commands.ModeOposer; import frc.robot.commands.MonterBalyeuse; import frc.robot.commands.MonterGrimpeur; @@ -36,6 +37,7 @@ public class RobotContainer { private void configureBindings() { manette.a().whileTrue(new Lancer(lanceur,limeLight3,balayeuse,led)); manette.y().whileTrue(new ModeOposer(lanceur, balayeuse)); + manette.x().whileTrue(new LancerBaseVitesse(lanceur, limeLight3, balayeuse, led)); manette.rightTrigger().whileTrue(new MonterGrimpeur(grimpeur)); manette.leftTrigger().whileTrue(new DescendreGrimpeur(grimpeur)); manette.rightBumper().whileTrue(new MonterBalyeuse(balayeuse)); diff --git a/src/main/java/frc/robot/commands/LancerBaseVitesse.java b/src/main/java/frc/robot/commands/LancerBaseVitesse.java new file mode 100644 index 0000000..9453127 --- /dev/null +++ b/src/main/java/frc/robot/commands/LancerBaseVitesse.java @@ -0,0 +1,91 @@ +// 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.commands; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.Timer; +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 */ +public class LancerBaseVitesse extends Command { + private Lanceur lanceur; + private PIDController pidController; + private Limelight3G limeLight3G; + private Timer timer; + private Balayeuse balayeuse; + private Led led; + private double temp; + /** Creates a new Lancer. */ + public LancerBaseVitesse(Lanceur lanceur, LimeLight3 limeLight3, Balayeuse balayeuse,Led led) { + this.lanceur = lanceur; + this.balayeuse = balayeuse; + this.led = led; + this.timer = new Timer(); + this.limeLight3G = new Limelight3G(); + addRequirements(lanceur, balayeuse, led, limeLight3G); + this.temp = 0; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + 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() { + 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 vitesse = 0.4; + double output = pidController.calculate(lanceur.Vitesse(),vitesse); + lanceur.Lancer(output); + if(lanceur.Vitesse() >= vitesse){ + lanceur.Demeler(0.5); + } + } + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + lanceur.Demeler(0); + lanceur.Lancer(0); + balayeuse.Pivoter(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +}