From 5b2606140fdc61ba9a6fb8da71463a2c3a421790 Mon Sep 17 00:00:00 2001 From: EdwardFaucher Date: Wed, 15 Mar 2023 20:26:42 -0400 Subject: [PATCH] modif debug --- src/main/java/frc/robot/RobotContainer.java | 15 ++++++++++----- src/main/java/frc/robot/commands/BrakeFerme.java | 2 +- src/main/java/frc/robot/commands/BrakeOuvre.java | 2 +- src/main/java/frc/robot/commands/Reculer.java | 2 +- .../java/frc/robot/commands/bras/BrasManuel.java | 5 ++++- .../java/frc/robot/commands/bras/FermePince.java | 2 +- .../java/frc/robot/commands/bras/OuvrePince.java | 2 +- .../frc/robot/commands/bras/PivotBrasRentre.java | 4 ++-- .../frc/robot/commands/bras/PivotChercheHaut.java | 4 ++-- .../java/frc/robot/commands/bras/PivotManuel.java | 3 ++- .../frc/robot/commands/bras/PivoteBrasBas.java | 7 ++++--- .../frc/robot/commands/bras/PivoteBrasHaut.java | 8 ++++---- .../robot/commands/bras/PivoteBrasMilieux.java | 13 +++++++------ 13 files changed, 40 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9c42ffa..38fcbe5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -65,9 +65,9 @@ ShuffleboardLayout layoutauto = Shuffleboard.getTab("teb").getLayout("auto",Buil .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("reculerdistancesortir",-66).getEntry(); +GenericEntry reculerdistanceb = layoutauto.addPersistent("reculerdistancebalance", -31).getEntry(); +GenericEntry avancerdistance = layoutauto.addPersistent("avancer",-35).getEntry(); // subsystems BasePilotable basePilotable = new BasePilotable(); Gratte gratte = new Gratte(); @@ -133,13 +133,14 @@ public RobotContainer() { manette2.start().onTrue(new InstantCommand(basePilotable::resetGyro)); manette2.a().whileTrue(gratteMonte); manette2.b().whileTrue(gratteBaisser); + manette2.back().onTrue(new InstantCommand(basePilotable::Reset)); } public Command getAutonomousCommand() { chooser.getSelected(); autobalance.getBoolean(true); - return new SequentialCommandGroup( + return Commands.waitSeconds(14).deadlineWith( new SequentialCommandGroup( Commands.select(Map.ofEntries( Map.entry(enhaut,pivoteBrasHaut), Map.entry(aumilieux,pivoteBrasMilieux), @@ -147,10 +148,14 @@ public RobotContainer() { Map.entry(nulpart,pivotBrasRentre) ), chooser::getSelected), ouvrePince.unless(()->chooser.getSelected().equals(nulpart)), + Commands.waitSeconds(1), + fermePince.unless(()->chooser.getSelected().equals(nulpart)), + new PivotBrasRentre(brasTelescopique , pivot).unless(()->chooser.getSelected().equals(nulpart)), + Commands.waitSeconds(1), Commands.either(reculers, reculerb,()-> autosortir.getBoolean(true)), avancer.unless(()->!autosortir.getBoolean(true)|| !autobalance.getBoolean(false)), Commands.either(gyro, Commands.none(),()-> autobalance.getBoolean(true)) - ).deadlineWith(Commands.waitSeconds(14.6)).andThen(brakeOuvre); + )).andThen(brakeOuvre); } diff --git a/src/main/java/frc/robot/commands/BrakeFerme.java b/src/main/java/frc/robot/commands/BrakeFerme.java index cd2be53..4e8fa49 100644 --- a/src/main/java/frc/robot/commands/BrakeFerme.java +++ b/src/main/java/frc/robot/commands/BrakeFerme.java @@ -37,6 +37,6 @@ public class BrakeFerme extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return true; } } diff --git a/src/main/java/frc/robot/commands/BrakeOuvre.java b/src/main/java/frc/robot/commands/BrakeOuvre.java index 96b0997..9ec5eb8 100644 --- a/src/main/java/frc/robot/commands/BrakeOuvre.java +++ b/src/main/java/frc/robot/commands/BrakeOuvre.java @@ -37,6 +37,6 @@ public class BrakeOuvre extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return true; } } diff --git a/src/main/java/frc/robot/commands/Reculer.java b/src/main/java/frc/robot/commands/Reculer.java index e9d245b..b19b241 100644 --- a/src/main/java/frc/robot/commands/Reculer.java +++ b/src/main/java/frc/robot/commands/Reculer.java @@ -30,7 +30,7 @@ public class Reculer extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - basePilotable.drive(-0.3,0); + basePilotable.drive(-0.5,0); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/bras/BrasManuel.java b/src/main/java/frc/robot/commands/bras/BrasManuel.java index 36b3535..b930d31 100644 --- a/src/main/java/frc/robot/commands/bras/BrasManuel.java +++ b/src/main/java/frc/robot/commands/bras/BrasManuel.java @@ -29,7 +29,10 @@ public class BrasManuel extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - brasTelescopique.AvanceRecule(doubleSupplier.getAsDouble()); + if(doubleSupplier.getAsDouble()>0.2 && doubleSupplier.getAsDouble()<-0.2){ + brasTelescopique.AvanceRecule(doubleSupplier.getAsDouble()); + } + } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/bras/FermePince.java b/src/main/java/frc/robot/commands/bras/FermePince.java index 609d44d..947a606 100644 --- a/src/main/java/frc/robot/commands/bras/FermePince.java +++ b/src/main/java/frc/robot/commands/bras/FermePince.java @@ -33,6 +33,6 @@ public class FermePince extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return true; } } diff --git a/src/main/java/frc/robot/commands/bras/OuvrePince.java b/src/main/java/frc/robot/commands/bras/OuvrePince.java index 2ad46c5..4f439e1 100644 --- a/src/main/java/frc/robot/commands/bras/OuvrePince.java +++ b/src/main/java/frc/robot/commands/bras/OuvrePince.java @@ -33,6 +33,6 @@ public class OuvrePince extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return true; } } diff --git a/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java b/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java index b35f2a8..46e0c0f 100644 --- a/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java +++ b/src/main/java/frc/robot/commands/bras/PivotBrasRentre.java @@ -42,7 +42,7 @@ public class PivotBrasRentre extends CommandBase { pivot.monteDescendre(0); } else{ - pivot.monteDescendre(-0.3); + pivot.monteDescendre(-0.5); } } @@ -57,6 +57,6 @@ public class PivotBrasRentre extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return brasTelescopique.photocell() && pivot.limitpivot(); } } diff --git a/src/main/java/frc/robot/commands/bras/PivotChercheHaut.java b/src/main/java/frc/robot/commands/bras/PivotChercheHaut.java index bd77d22..19bfcac 100644 --- a/src/main/java/frc/robot/commands/bras/PivotChercheHaut.java +++ b/src/main/java/frc/robot/commands/bras/PivotChercheHaut.java @@ -38,10 +38,10 @@ public class PivotChercheHaut extends CommandBase { brasTelescopique.AvanceRecule(0.3); } if (pivot.distance()<43.5){ - pivot.monteDescendre(0.4); + pivot.monteDescendre(0.5); } else if(pivot.distance()>44.5) { - pivot.monteDescendre(-0.4); + pivot.monteDescendre(-0.5); } else{ pivot.monteDescendre(0); diff --git a/src/main/java/frc/robot/commands/bras/PivotManuel.java b/src/main/java/frc/robot/commands/bras/PivotManuel.java index 53576af..375ce9c 100644 --- a/src/main/java/frc/robot/commands/bras/PivotManuel.java +++ b/src/main/java/frc/robot/commands/bras/PivotManuel.java @@ -27,7 +27,8 @@ public class PivotManuel extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - pivot.monteDescendre(doubleSupplier.getAsDouble()); + if(doubleSupplier.getAsDouble()>0.2 && doubleSupplier.getAsDouble()<-0.2){ + pivot.monteDescendre(doubleSupplier.getAsDouble());} } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java b/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java index 4634d75..130619b 100644 --- a/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java +++ b/src/main/java/frc/robot/commands/bras/PivoteBrasBas.java @@ -43,10 +43,10 @@ public class PivoteBrasBas extends CommandBase { brasTelescopique.ouvrir(); } if (pivot.distance()<8.5){ - pivot.monteDescendre(0.3); + pivot.monteDescendre(0.4); } else if(pivot.distance()>10.5) { - pivot.monteDescendre(-0.3); + pivot.monteDescendre(-0.4); } else{ pivot.monteDescendre(0); @@ -65,6 +65,7 @@ public class PivoteBrasBas extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return brasTelescopique.distance()>-13.5 && brasTelescopique.distance() <-15.5 && pivot.distance()<8.5 && pivot.distance()>10.5; + } } diff --git a/src/main/java/frc/robot/commands/bras/PivoteBrasHaut.java b/src/main/java/frc/robot/commands/bras/PivoteBrasHaut.java index cf41d39..c670121 100644 --- a/src/main/java/frc/robot/commands/bras/PivoteBrasHaut.java +++ b/src/main/java/frc/robot/commands/bras/PivoteBrasHaut.java @@ -33,7 +33,7 @@ public class PivoteBrasHaut extends CommandBase { brasTelescopique.AvanceRecule(-0.15); brasTelescopique.fermer(); } - else if(brasTelescopique.distance()<41.5) { + else if(brasTelescopique.distance()<-41.5) { brasTelescopique.AvanceRecule(-0.15); } else { @@ -41,10 +41,10 @@ public class PivoteBrasHaut extends CommandBase { brasTelescopique.ouvrir(); } if (pivot.distance()<50.5){ - pivot.monteDescendre(0.4); + pivot.monteDescendre(0.5); } else if(pivot.distance()>52.5) { - pivot.monteDescendre(-0.4); + pivot.monteDescendre(-0.5); } else{ pivot.monteDescendre(0); @@ -62,6 +62,6 @@ public class PivoteBrasHaut extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return brasTelescopique.distance()>-39.5 && brasTelescopique.distance() <-41.5 && pivot.distance()<50.5 && pivot.distance()>52.5; } } diff --git a/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java b/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java index f5abb78..764050b 100644 --- a/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java +++ b/src/main/java/frc/robot/commands/bras/PivoteBrasMilieux.java @@ -30,21 +30,21 @@ public class PivoteBrasMilieux extends CommandBase { @Override public void execute() { if(brasTelescopique.distance()>-16.5){ - brasTelescopique.AvanceRecule(-0.2); + brasTelescopique.AvanceRecule(-0.15); brasTelescopique.fermer(); } else if(brasTelescopique.distance()<-17.5) { - brasTelescopique.AvanceRecule(0.2); + brasTelescopique.AvanceRecule(0.15); } else { brasTelescopique.AvanceRecule(0); brasTelescopique.ouvrir(); } if (pivot.distance()<43.5){ - pivot.monteDescendre(0.4); + pivot.monteDescendre(0.6); } else if(pivot.distance()>44.5) { - pivot.monteDescendre(-0.4); + pivot.monteDescendre(-0.6); } else{ pivot.monteDescendre(0); @@ -63,6 +63,7 @@ public class PivoteBrasMilieux extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return brasTelescopique.distance()>-16.5 && brasTelescopique.distance() <-15.5 && pivot.distance()<44.5 && pivot.distance()>43.5; + } } -} +