Merge branch 'main' of https://demerso.net/pls5618/2023/robot

src/main/java/frc/robot/RobotContainer.java

@@ -6,17 +6,18 @@ package frc.robot;
 
 import java.util.Map;
 
-
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.networktables.GenericEntry;
 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;
@@ -40,8 +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.Bougerbras;
-import frc.robot.commands.bras.Bougerpivot;
+
 import frc.robot.commands.bras.BrasManuel;
 import frc.robot.commands.bras.DescendrePivotBras;
 import frc.robot.commands.bras.FermePince;
@@ -49,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);
@@ -64,9 +65,13 @@ 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", -66).getEntry();
-  GenericEntry reculerdistanceb = layoutauto.addPersistent("reculerdistancebalance", -31).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();
   Gratte gratte = new Gratte();
@@ -80,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());
@@ -111,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);
@@ -121,14 +126,14 @@ 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);
     manette2.leftBumper().whileTrue(cone);
     manette2.y().whileTrue(gyro);
     manette2.start().onTrue(new InstantCommand(basePilotable::resetGyro));
-    manette1.b().whileTrue(gratteMonte);
+    manette1.x().whileTrue(gratteMonte);
     manette1.x().whileTrue(gratteBaisser);
 
   }
@@ -148,18 +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(49, 40).unless(() -> chooser.getSelected().equals(enhaut)),
-               creerCommandBras(51, 40).unless(() -> chooser.getSelected().equals(enhaut)),
+            // 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 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);
 

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.

src/main/java/frc/robot/commands/Gyro.java

@@ -5,14 +5,18 @@
 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);
   }
@@ -26,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
    {

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.

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.
   }

src/main/java/frc/robot/commands/bras/Bougerpivot.java

@@ -15,6 +15,7 @@ public class Bougerpivot extends CommandBase {
     this.pivot = pivot;
     this.distance = distance;
     addRequirements(pivot);
+    
     // Use addRequirements() here to declare subsystem dependencies.
   }
 

src/main/java/frc/robot/commands/bras/DescendrePivotBras.java

@@ -4,7 +4,6 @@
 
 package frc.robot.commands.bras;
 
-
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.subsystems.bras.BrasTelescopique;
 import frc.robot.subsystems.bras.Pivot;

src/main/java/frc/robot/commands/bras/MonterPivotBras.java

@@ -5,6 +5,7 @@
 package frc.robot.commands.bras;
 
 
+
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.subsystems.bras.BrasTelescopique;

src/main/java/frc/robot/subsystems/BasePilotable.java

@@ -74,6 +74,7 @@ public void BrakeFerme(){
     teb.add(drive);
     teb.addDouble("distancerobot",this::distance);
     teb.addDouble("angle gyro", this::getpitch);
+    
   }
 
   @Override