From 1da5d821c35f8ef50b93f3c56ada9364a1925ba3 Mon Sep 17 00:00:00 2001
From: samuel desharnais <samdes330@gmail.com>
Date: Mon, 20 Mar 2023 18:00:49 -0400
Subject: [PATCH] g

---
 src/main/java/frc/robot/RobotContainer.java | 36 +++++++++------------
 1 file changed, 16 insertions(+), 20 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index f151825..a68925a 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",-66).getEntry();
-GenericEntry reculerdistanceb = layoutauto.addPersistent("reculerdistancebalance", -31).getEntry();
-GenericEntry avancerdistance = layoutauto.addPersistent("avancer",-35).getEntry();
+GenericEntry reculerdistances = layoutauto.addPersistent("reculerdistancesortir",0).getEntry();
+GenericEntry reculerdistanceb = layoutauto.addPersistent("reculerdistancebalance", 0).getEntry();
+GenericEntry avancerdistance = layoutauto.addPersistent("avancer",0).getEntry();
 // subsystems
 BasePilotable basePilotable = new BasePilotable();
 Gratte gratte = new Gratte();
@@ -93,9 +93,9 @@ Reculer reculerb = new Reculer(basePilotable, ()->reculerdistanceb.getDouble(0))
 Avancer avancer = new Avancer(basePilotable, ()->avancerdistance.getDouble(0));
 PivotChercheBas pivotChercheBas = new PivotChercheBas(brasTelescopique, pivot);
 PivotChercheHaut pivotChercheHaut = new PivotChercheHaut(brasTelescopique, pivot);
-Cube cube = new Cube(limelight, basePilotable, ()-> manette1.getLeftY());
-Apriltag aprilTag = new Apriltag(limelight, basePilotable,()-> manette1.getLeftY());
-Tape tape = new Tape(limelight, basePilotable, ()-> manette1.getLeftY());
+Cube cube = new Cube(limelight, basePilotable, null);
+Apriltag aprilTag = new Apriltag(limelight, basePilotable, null);
+Tape tape = new Tape(limelight, basePilotable, null);
 PivotManuel pivotManuel = new PivotManuel(pivot,manette2::getLeftX);
 BrasManuel brasManuel = new BrasManuel(brasTelescopique,manette2::getLeftY);
 
@@ -126,10 +126,9 @@ public RobotContainer() {
   private void configureBindings() {
     // manette 1
     manette1.a().toggleOnTrue(Commands.startEnd(pince::ouvrir, pince::fermer,pince));
-    manette1.x().onTrue(brakeOuvre);
-    manette1.b().onTrue(brakeFerme);
-    manette1.leftBumper().whileTrue(aprilTag);
-    manette1.rightBumper().whileTrue(tape);
+    manette1.x().toggleOnTrue(Commands.startEnd(basePilotable::BrakeFerme,basePilotable::BrakeOuvre,basePilotable));
+    manette1.leftBumper().toggleOnTrue(aprilTag);
+    manette1.rightBumper().toggleOnTrue(tape);
     manette1.povUp().whileTrue(pivoteBrasHaut);
     manette1.povDown().whileTrue(pivoteBrasBas);
     manette1.povRight().whileTrue(pivoteBrasMilieux);
@@ -137,18 +136,19 @@ public RobotContainer() {
     //manette 2
     manette2.povDown().onTrue(pivotChercheBas);
     manette2.povUp().onTrue(pivotChercheHaut);
-    manette2.rightBumper().whileTrue(cube);
-    manette2.leftBumper().whileTrue(cone);
+    manette2.rightBumper().toggleOnTrue(cube);
+    manette2.leftBumper().toggleOnTrue(cone);
     manette2.y().whileTrue(gyro);
     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() {
-    return Commands.waitSeconds(14).deadlineWith( new SequentialCommandGroup(
+    chooser.getSelected();
+    autobalance.getBoolean(true);
+    return new SequentialCommandGroup(
       Commands.select(Map.ofEntries(
         Map.entry(enhaut,pivoteBrasHaut),
         Map.entry(aumilieux,pivoteBrasMilieux),
@@ -156,14 +156,10 @@ 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)),
+      avancer.unless(()->!autosortir.getBoolean(true)|| !autobalance.getBoolean(false)),
       Commands.either(gyro, Commands.none(),()-> autobalance.getBoolean(true))
-    )).andThen(new InstantCommand(basePilotable::BrakeOuvre,basePilotable));
+    ).deadlineWith(Commands.waitSeconds(14.6)).andThen(brakeOuvre);
 
 
   }