diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1953f56..f9dd1fd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,10 +12,12 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; 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.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -39,6 +41,7 @@ import frc.robot.commands.GratteMonte; import frc.robot.commands.Gyro; import frc.robot.commands.Reculer; import frc.robot.commands.Tape; + import frc.robot.commands.bras.BrasManuel; import frc.robot.commands.bras.DescendrePivotBras; import frc.robot.commands.bras.FermePince; @@ -46,6 +49,7 @@ import frc.robot.commands.bras.MonterPivotBras; import frc.robot.commands.bras.OuvrePince; import frc.robot.commands.bras.PivotManuel; import frc.robot.commands.ActiverLimeLight; + public class RobotContainer { CommandXboxController manette1 = new CommandXboxController(0); @@ -61,9 +65,12 @@ public class RobotContainer { .withSize(2, 2).withProperties(Map.of("Label position", "LEFT")); GenericEntry autobalance = layoutauto.add("choix balance", true).withWidget(BuiltInWidgets.kToggleSwitch).getEntry(); GenericEntry autosortir = layoutauto.add("choix sortir", false).withWidget(BuiltInWidgets.kToggleSwitch).getEntry(); - GenericEntry reculerdistances = layoutauto.addPersistent("reculerdistancesortir", 0).getEntry(); - GenericEntry reculerdistanceb = layoutauto.addPersistent("reculerdistancebalance", 0).getEntry(); - GenericEntry avancerdistance = layoutauto.addPersistent("avancer", 0).getEntry(); + GenericEntry reculerdistances = layoutauto.addPersistent("reculer distance sortir", -66).getEntry(); + GenericEntry reculerdistanceb = layoutauto.addPersistent("reculer distance balance", -31).getEntry(); + GenericEntry avancerdistance = layoutauto.addPersistent("avancer", 35).getEntry(); + GenericEntry avancerforce = layoutauto.addPersistent("avancer force", 0.4).getEntry(); + GenericEntry reculerforce = layoutauto.addPersistent("reculer force", -0.45).getEntry(); + GenericEntry gyroforce = layoutauto.addPersistent("gyro force", 0.3).getEntry(); // subsystems BasePilotable basePilotable = new BasePilotable(); @@ -78,13 +85,13 @@ public class RobotContainer { BrakeOuvre brakeOuvre = new BrakeOuvre(basePilotable); GratteBaisser gratteBaisser = new GratteBaisser(gratte); GratteMonte gratteMonte = new GratteMonte(gratte); - Gyro gyro = new Gyro(basePilotable); + Gyro gyro = new Gyro(basePilotable, () -> gyroforce.getDouble(0)); FermePince fermePince = new FermePince(pince); OuvrePince ouvrePince = new OuvrePince(pince); Cone cone = new Cone(limelight, basePilotable, () -> -manette1.getLeftY()); - Reculer reculers = new Reculer(basePilotable, () -> reculerdistances.getDouble(0)); - Reculer reculerb = new Reculer(basePilotable, () -> reculerdistanceb.getDouble(0)); - Avancer avancer = new Avancer(basePilotable, () -> avancerdistance.getDouble(0)); + Reculer reculers = new Reculer(basePilotable, () -> reculerdistances.getDouble(0), () -> reculerforce.getDouble(0)); + Reculer reculerb = new Reculer(basePilotable, () -> reculerdistanceb.getDouble(0), () -> reculerforce.getDouble(0)); + Avancer avancer = new Avancer(basePilotable, () -> avancerdistance.getDouble(0), () -> avancerforce.getDouble(0)); Cube cube = new Cube(limelight, basePilotable, () -> manette1.getLeftY()); Apriltag aprilTag = new Apriltag(limelight, basePilotable, () -> manette1.getLeftY()); Tape tape = new Tape(limelight, basePilotable, () -> manette1.getLeftY()); @@ -109,7 +116,7 @@ public class RobotContainer { private void configureBindings() { // manette 1 - manette1.a().toggleOnTrue(Commands.startEnd(pince::ouvrir, pince::fermer,pince)); + manette1.a().toggleOnTrue(Commands.startEnd(pince::ouvrir, pince::fermer, pince)); manette2.a().onTrue(brakeOuvre); manette2.b().onTrue(brakeFerme); manette1.leftBumper().whileTrue(aprilTag); @@ -119,7 +126,7 @@ public class RobotContainer { manette1.povRight().whileTrue(creerCommandBras(44, -17)); manette1.povLeft().whileTrue(creerCommandBras(0, 0)); manette1.y().whileTrue(activerLimeLight); - //manette 2 + // manette 2 manette2.povDown().whileTrue(creerCommandBras(9, -18)); manette2.povUp().whileTrue(creerCommandBras(44, 0)); manette2.rightBumper().whileTrue(cube); @@ -146,16 +153,20 @@ public class RobotContainer { new SequentialCommandGroup( Commands.select(Map.ofEntries( Map.entry(enhaut, creerCommandBras(51, -40)), - Map.entry(aumilieux, creerCommandBras(9, -14)), - Map.entry(enbas, creerCommandBras(44, -17)), + Map.entry(aumilieux, creerCommandBras(45, -13)), + Map.entry(enbas, creerCommandBras(12, -9)), Map.entry(nulpart, creerCommandBras(0, 0))), chooser::getSelected), + // creerCommandBras(50, -40).unless(() -> + // !chooser.getSelected().equals(enhaut)), + // creerCommandBras(51, -40).unless(() -> + // !chooser.getSelected().equals(enhaut)), new OuvrePince(pince).unless(() -> chooser.getSelected().equals(nulpart)), Commands.waitSeconds(1), - new FermePince(pince).unless(() -> chooser.getSelected().equals(nulpart)), - creerCommandBras(0, 0).unless(() -> chooser.getSelected().equals(nulpart)), - Commands.waitSeconds(1), - Commands.either(reculers, reculerb, () -> autosortir.getBoolean(true)), - new Avancer(basePilotable, () -> avancerdistance.getDouble(0)).unless(() -> !autosortir.getBoolean(true) || !autobalance.getBoolean(false)), + new FermePince(pince).unless(() -> chooser.getSelected().equals(nulpart)), + new ParallelCommandGroup(creerCommandBras(0, 0).unless(() -> chooser.getSelected().equals(nulpart)), + Commands.either(reculers, reculerb, () -> autosortir.getBoolean(true))), + new Avancer(basePilotable, () -> avancerdistance.getDouble(0), () -> avancerforce.getDouble(0)) + .unless(() -> !autosortir.getBoolean(true) || !autobalance.getBoolean(false)), Commands.either(gyro, Commands.none(), () -> autobalance.getBoolean(true)))) .andThen(brakeOuvre); diff --git a/src/main/java/frc/robot/commands/Avancer.java b/src/main/java/frc/robot/commands/Avancer.java index f7d37a1..f62d7f9 100644 --- a/src/main/java/frc/robot/commands/Avancer.java +++ b/src/main/java/frc/robot/commands/Avancer.java @@ -13,10 +13,12 @@ import frc.robot.subsystems.BasePilotable; public class Avancer extends CommandBase { BasePilotable basePilotable; DoubleSupplier distance; + DoubleSupplier force; /** Creates a new Reculer. */ - public Avancer(BasePilotable basePilotable, DoubleSupplier distance) { + public Avancer(BasePilotable basePilotable, DoubleSupplier distance, DoubleSupplier force) { this.basePilotable = basePilotable; this.distance = distance; + this.force = force; // Use addRequirements() here to declare subsystem dependencies. addRequirements(basePilotable); } @@ -30,7 +32,7 @@ public class Avancer extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - basePilotable.drive(0.4,0); + basePilotable.drive(force.getAsDouble(),0); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/GratteMonte.java b/src/main/java/frc/robot/commands/GratteMonte.java index 5dce6f8..d72480b 100644 --- a/src/main/java/frc/robot/commands/GratteMonte.java +++ b/src/main/java/frc/robot/commands/GratteMonte.java @@ -5,6 +5,7 @@ package frc.robot.commands; + import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Gratte; diff --git a/src/main/java/frc/robot/commands/Gyro.java b/src/main/java/frc/robot/commands/Gyro.java index 5d6b948..ea56e0d 100644 --- a/src/main/java/frc/robot/commands/Gyro.java +++ b/src/main/java/frc/robot/commands/Gyro.java @@ -4,14 +4,19 @@ package frc.robot.commands; + +import java.util.function.DoubleSupplier; + import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.BasePilotable; public class Gyro extends CommandBase { private BasePilotable basePilotable; + DoubleSupplier force; /** Creates a new Gyro. */ - public Gyro(BasePilotable basePilotable) { + public Gyro(BasePilotable basePilotable,DoubleSupplier force) { this.basePilotable = basePilotable; + this.force = force; // Use addRequirements() here to declare subsystem dependencies. addRequirements(basePilotable); } @@ -25,11 +30,11 @@ public class Gyro extends CommandBase { public void execute() { if(basePilotable.getpitch()>6) { - basePilotable.drive(0.4*basePilotable.getpitch()/12, 0); + basePilotable.drive(force.getAsDouble()*basePilotable.getpitch()/12, 0); } else if(basePilotable.getpitch()<-6) { - basePilotable.drive(0.4*basePilotable.getpitch()/12, 0); + basePilotable.drive(force.getAsDouble()*basePilotable.getpitch()/12, 0); } else { diff --git a/src/main/java/frc/robot/commands/Reculer.java b/src/main/java/frc/robot/commands/Reculer.java index 903b216..128cdfb 100644 --- a/src/main/java/frc/robot/commands/Reculer.java +++ b/src/main/java/frc/robot/commands/Reculer.java @@ -13,10 +13,12 @@ import frc.robot.subsystems.BasePilotable; public class Reculer extends CommandBase { BasePilotable basePilotable; DoubleSupplier distance; + DoubleSupplier force; /** Creates a new Reculer. */ - public Reculer(BasePilotable basePilotable, DoubleSupplier distance) { + public Reculer(BasePilotable basePilotable, DoubleSupplier distance,DoubleSupplier force) { this.basePilotable = basePilotable; this.distance = distance; + this.force = force; // Use addRequirements() here to declare subsystem dependencies. addRequirements(basePilotable); } @@ -30,7 +32,7 @@ public class Reculer extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - basePilotable.drive(-0.4,0); + basePilotable.drive(force.getAsDouble(),0); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/bras/Bougerbras.java b/src/main/java/frc/robot/commands/bras/Bougerbras.java index 6390c94..20c3a99 100644 --- a/src/main/java/frc/robot/commands/bras/Bougerbras.java +++ b/src/main/java/frc/robot/commands/bras/Bougerbras.java @@ -11,9 +11,11 @@ public class Bougerbras extends CommandBase { /** Creates a new bougerbras. */ double distance; BrasTelescopique brasTelescopique; + public Bougerbras(BrasTelescopique brasTelescopique,double distance) { this.brasTelescopique = brasTelescopique; this.distance = distance; + addRequirements(brasTelescopique); // Use addRequirements() here to declare subsystem dependencies. } @@ -28,11 +30,11 @@ public class Bougerbras extends CommandBase { @Override public void execute() { if(brasTelescopique.distance()>distance+0.5 ) { - brasTelescopique.AvanceRecule(-0.3); + brasTelescopique.AvanceRecule(-0.35); } else if(brasTelescopique.distance()