diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f0e2a60..28536f3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,15 +15,16 @@ 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.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.Aspirer; import frc.robot.commands.DescendreBalyeuse; import frc.robot.commands.DescendreGrimpeur; import frc.robot.commands.Lancer; +import frc.robot.commands.LancerAspirer; import frc.robot.commands.LancerBaseVitesse; import frc.robot.commands.Limelighter; import frc.robot.commands.ModeOposer; +import frc.robot.commands.ModeOposerDemeleur; import frc.robot.commands.MonterBalyeuse; import frc.robot.commands.MonterGrimpeur; import frc.robot.commands.ModeAuto.AspirerAuto; @@ -52,6 +53,7 @@ public class RobotContainer { Limelight3G limeLight3G = new Limelight3G(); Led led = new Led(); CommandXboxController manette = new CommandXboxController(0); + CommandXboxController manette1 = new CommandXboxController(1); private double MaxSpeed = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity @@ -93,15 +95,21 @@ public class RobotContainer { .withRotationalRate(MathUtil.applyDeadband(-manette.getRightX()*Math.abs(-manette.getRightX())*MaxAngularRate, 0.05)) ) ); + //manette 1 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)); + manette.x().whileTrue(new LancerBaseVitesse(lanceur, limeLight3)); manette.rightTrigger().whileTrue(new MonterGrimpeur(grimpeur)); manette.leftTrigger().whileTrue(new DescendreGrimpeur(grimpeur)); manette.rightBumper().whileTrue(new MonterBalyeuse(balayeuse)); manette.leftBumper().whileTrue(new DescendreBalyeuse(balayeuse)); + manette.b().whileTrue(new Aspirer(balayeuse,led)); + //manette 2 + + manette1.povDown().whileTrue(new LancerAspirer(lanceur, balayeuse, limeLight3, led)); + manette1.y().whileTrue(new ModeOposer(lanceur, balayeuse)); + manette1.x().whileTrue(new ModeOposerDemeleur(lanceur)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/Lancer.java b/src/main/java/frc/robot/commands/Lancer.java index c4a3db4..7d3677f 100644 --- a/src/main/java/frc/robot/commands/Lancer.java +++ b/src/main/java/frc/robot/commands/Lancer.java @@ -82,7 +82,7 @@ public class Lancer extends Command { double output = vitesse; /*pidController.calculate(lanceur.Vitesse(),vitesse);*/ lanceur.Lancer(output); if(lanceur.Vitesse() >= vitesse){ - lanceur.Demeler(0.5); + lanceur.Demeler(0.1); } // } diff --git a/src/main/java/frc/robot/commands/LancerAspirer.java b/src/main/java/frc/robot/commands/LancerAspirer.java new file mode 100644 index 0000000..9b9967f --- /dev/null +++ b/src/main/java/frc/robot/commands/LancerAspirer.java @@ -0,0 +1,25 @@ +// 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.wpilibj2.command.ParallelCommandGroup; +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; + + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class LancerAspirer extends ParallelCommandGroup { + /** Creates a new LacerAspirer. */ + public LancerAspirer(Lanceur lanceur, Balayeuse balayeuse, LimeLight3 limeLight3, Led led) { + addCommands(new LancerBaseVitesse(lanceur, limeLight3), new Aspirer(balayeuse, led)); + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + } +} diff --git a/src/main/java/frc/robot/commands/LancerBaseVitesse.java b/src/main/java/frc/robot/commands/LancerBaseVitesse.java index 44e88e0..ccf8a34 100644 --- a/src/main/java/frc/robot/commands/LancerBaseVitesse.java +++ b/src/main/java/frc/robot/commands/LancerBaseVitesse.java @@ -7,9 +7,7 @@ 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; @@ -19,28 +17,24 @@ public class LancerBaseVitesse extends Command { 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) { + public LancerBaseVitesse(Lanceur lanceur, LimeLight3 limeLight3) { this.lanceur = lanceur; - this.balayeuse = balayeuse; - this.led = led; - this.timer = new Timer(); + // this.timer = new Timer(); this.limeLight3G = new Limelight3G(); - addRequirements(lanceur, balayeuse, led, limeLight3G); - this.temp = 0; + addRequirements(lanceur, 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(1, 0,0, 0); - timer.reset(); - timer.start(); - temp = lanceur.Amp(); + pidController = new PIDController(0.0007, 0,0, 0.001); + //timer.reset(); + //timer.start(); + //temp = lanceur.Amp(); } // Called every time the scheduler runs while the command is scheduled. @@ -65,10 +59,10 @@ public class LancerBaseVitesse extends Command { // led.Jaune2(); // } // else{ - double vitesse = 0.4; + double vitesse = lanceur.vitesseDemander(); double output = pidController.calculate(lanceur.Vitesse(),vitesse); lanceur.Lancer(output); - if(lanceur.Vitesse() >= vitesse){ + if(lanceur.Vitesse() >= 1200){ lanceur.Demeler(0.5); } // } @@ -80,7 +74,6 @@ public class LancerBaseVitesse extends Command { public void end(boolean interrupted) { lanceur.Demeler(0); lanceur.Lancer(0); - balayeuse.Pivoter(0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java b/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java index 749233a..49740af 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java +++ b/src/main/java/frc/robot/commands/ModeAuto/LancerAuto.java @@ -15,6 +15,7 @@ public class LancerAuto extends Command { /** Creates a new LancerAuto. */ public LancerAuto(Lanceur lanceur) { this.lanceur = lanceur; + timer = new Timer(); addRequirements(lanceur); // Use addRequirements() here to declare subsystem dependencies. } @@ -29,9 +30,9 @@ public class LancerAuto extends Command { @Override public void execute() { lanceur.Lancer(0.5); - if(timer.get() > 1){ - lanceur.Demeler(0.5); - } + if(timer.get() > 1){ + lanceur.Demeler(0.1); + } } // Called once the command ends or is interrupted. @@ -47,5 +48,6 @@ public class LancerAuto extends Command { @Override public boolean isFinished() { return timer.get() > 3; + //return false; } } diff --git a/src/main/java/frc/robot/commands/ModeOposer.java b/src/main/java/frc/robot/commands/ModeOposer.java index 068afc8..dc69a1e 100644 --- a/src/main/java/frc/robot/commands/ModeOposer.java +++ b/src/main/java/frc/robot/commands/ModeOposer.java @@ -29,7 +29,6 @@ public class ModeOposer extends Command { public void execute() { lanceur.Lancer(-0.2); lanceur.Demeler(-0.2); - balayeuse.Balayer(-0.2); } // Called once the command ends or is interrupted. @@ -37,7 +36,6 @@ public class ModeOposer extends Command { public void end(boolean interrupted) { lanceur.Demeler(0); lanceur.Lancer(0); - balayeuse.Balayer(0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/ModeOposerDemeleur.java b/src/main/java/frc/robot/commands/ModeOposerDemeleur.java new file mode 100644 index 0000000..940308a --- /dev/null +++ b/src/main/java/frc/robot/commands/ModeOposerDemeleur.java @@ -0,0 +1,42 @@ +// 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.wpilibj2.command.Command; +import frc.robot.subsystems.Lanceur; +/* 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 ModeOposerDemeleur extends Command { + private Lanceur lanceur; + /** Creates a new Lancer. */ + public ModeOposerDemeleur(Lanceur lanceur) { + this.lanceur = lanceur; + addRequirements(lanceur); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + lanceur.Demeler(-0.2); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + lanceur.Demeler(0); + lanceur.Lancer(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Lanceur.java b/src/main/java/frc/robot/subsystems/Lanceur.java index 3aee063..44a7aba 100644 --- a/src/main/java/frc/robot/subsystems/Lanceur.java +++ b/src/main/java/frc/robot/subsystems/Lanceur.java @@ -18,11 +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",0.5).getEntry(); + teb.add("vitesse lanceur",4000).getEntry(); GenericEntry AmpLanceur = teb.add("ampérage lanceur",30).getEntry(); + public void Lancer(double vitesse){ - // moteur1.set(-vitesse); + moteur1.set(vitesse); moteur2.set(vitesse); } public void Demeler(double vitesse){ @@ -38,11 +39,12 @@ public class Lanceur extends SubsystemBase { return AmpLanceur.getDouble(30); } public double vitesseDemander(){ - return vitesse.getDouble(0.5); + return vitesse.getDouble(4000); } /** Creates a new Lanceur. */ public Lanceur() { teb.addDouble("amperage lanceur", this::Amp); + teb.addDouble("vitesse actuelle",this::Vitesse); } @Override