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

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

@@ -3,11 +3,16 @@
 // the WPILib BSD license file in the root directory of this project.
 
 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.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
+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.RunCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
 //subsystems
@@ -25,7 +30,6 @@ import frc.robot.commands.Cone;
 import frc.robot.commands.GratteBaisser;
 import frc.robot.commands.GratteMonte;
 import frc.robot.commands.Gyro;
-import frc.robot.commands.Reculer;
 import frc.robot.commands.bras.FermePince;
 import frc.robot.commands.bras.OuvrePince;
 import frc.robot.commands.bras.PivotBrasRentre;
@@ -33,7 +37,6 @@ import frc.robot.commands.bras.PivoteBrasBas;
 import frc.robot.commands.bras.PivoteBrasHaut;
 import frc.robot.commands.bras.PivoteBrasMilieux;
 //subsystems
-import frc.robot.subsystems.BasePilotable;
 import frc.robot.commands.bras.PivotChercheBas;
 import frc.robot.commands.bras.PivotChercheHaut;
 import frc.robot.commands.Cube;
@@ -41,7 +44,11 @@ import frc.robot.commands.Tape;
 
 
 public class RobotContainer {
+<<<<<<< HEAD
+  
+=======
 //CameraServer.startAutomaticCapture();
+>>>>>>> fc4607f3c6d7203a155182eded4626d37f75de5e
 CommandXboxController manette1 = new CommandXboxController(0);
 CommandXboxController manette2 = new CommandXboxController(1);
 // subsystems
@@ -64,6 +71,16 @@ PivoteBrasBas pivoteBrasBas = new PivoteBrasBas(brasTelescopique, pivot);
 PivoteBrasMilieux pivoteBrasMilieux = new PivoteBrasMilieux(brasTelescopique, pivot);
 PivoteBrasHaut pivoteBrasHaut = new PivoteBrasHaut(brasTelescopique, pivot);
 Cone cone = new Cone(limelight, basePilotable, ()->-manette1.getLeftY());
+Reculer reculer = new Reculer(basePilotable);
+
+SendableChooser<String> chooser = new SendableChooser<>();
+String enhaut = "en bas";
+String aumilieux = "au milieux";
+String enbas = "en bas";
+String nulpart = "nul part";
+ShuffleboardLayout layoutauto = Shuffleboard.getTab("teb").getLayout("auto");
+GenericEntry autobalance = layoutauto.add("choix balance",true).getEntry();
+GenericEntry autosortir = layoutauto.add("choix sorit",false).getEntry();
 PivotChercheBas pivotChercheBas = new PivotChercheBas(brasTelescopique, pivot);
 PivotChercheHaut pivotChercheHaut = new PivotChercheHaut(brasTelescopique, pivot);
 Cube cube = new Cube(limelight, basePilotable, null);
@@ -71,16 +88,25 @@ Apriltag aprilTag = new Apriltag(limelight, basePilotable, null);
 Tape tape = new Tape(limelight, basePilotable, null);
 
 public RobotContainer() {
+    chooser.setDefaultOption(enhaut, enhaut);
+    chooser.addOption(enbas, enbas);
+    chooser.addOption(aumilieux, aumilieux);
+    chooser.addOption(nulpart, nulpart);
+    layoutauto.add("choix hauteur",chooser);
+
+
     configureBindings();
     CameraServer.startAutomaticCapture();
     basePilotable.setDefaultCommand(new RunCommand(() -> {
-      basePilotable.drive(-manette1.getLeftY(), manette1.getLeftX());
+      basePilotable.drive(-manette1.getLeftY(), manette1.getLeftX(), 0);
     },basePilotable));
     
 }
   
   private void configureBindings() {
- 
+    basePilotable.setDefaultCommand(new RunCommand(() -> {
+      basePilotable.drive(-manette1.getLeftY(), -manette1.getLeftX(), 0);
+    },basePilotable));
     // manette 1
     manette1.povDown().onTrue(pivoteBrasHaut);
     manette1.povUp().onTrue(pivoteBrasBas);
@@ -88,6 +114,38 @@ public RobotContainer() {
     manette1.povRight().onTrue(pivotBrasRentre);
     manette1.a().toggleOnTrue(Commands.startEnd(pince::ouvrir, pince::fermer,pince));
     manette1.x().toggleOnTrue(Commands.startEnd(basePilotable::BrakeFerme,basePilotable::BrakeOuvre,basePilotable));
+<<<<<<< HEAD
+    manette1.y().whileTrue(gyro);
+    manette1.start().toggleOnTrue(Commands.startEnd(basePilotable::resetGyro, basePilotable::resetGyro, basePilotable));
+    /*manette1.b().toggleOnTrue(Commands.startEnd(gratte::baiser, gratte::Lever,gratte));
+    manette2.a().toggleOnTrue(Commands.startEnd(brasTelescopique::pivoteBrasHaut, brasTelescopique::pivoteBrasHaut));
+    manette2.b().toggleOnTrue(Commands.startEnd(brasTelescopique::pivoteBrasBas, brasTelescopique::pivoteBrasBas, brasTelescopique));
+    manette2.x().toggleOnTrue(Commands.startEnd(brasTelescopique::pivoteBrasMilieux, brasTelescopique::pivoteBrasMilieux, brasTelescopique));
+    manette2.y().toggleOnTrue(Commands.startEnd(brasTelescopique::pivotBrasRentre, brasTelescopique::pivotBrasRentre, brasTelescopique));
+    manette2.povRight().toggleOnTrue(Commands.startEnd(brasTelescopique::PivotChercheBas, brasTelescopique::PivotChercheBas, brasTelescopique));
+    manette2.povLeft().toggleOnTrue(Commands.startEnd(brasTelescopique::PivotChercheHaut, brasTelescopique::PivotChercheHaut, brasTelescopique));
+    manette2.rightBumper().toggleOnTrue(Commands.startEnd(null, null, null));
+    manette2.leftBumper().toggleOnTrue(Commands.startEnd(null, null, null));**/
+    manette2.leftBumper().toggleOnTrue(Commands.startEnd(null, null, null));
+    
+  }
+
+  public Command getAutonomousCommand() {
+    chooser.getSelected();
+    autobalance.getBoolean(true);
+    return new SequentialCommandGroup(
+      Commands.either(gyro, Commands.none(),()-> autobalance.getBoolean(true)),
+      Commands.either(reculer, Commands.none(),()-> autosortir.getBoolean(true)),
+      Commands.select(Map.ofEntries(
+        Map.entry(enhaut,pivoteBrasHaut),
+        Map.entry(aumilieux,null),
+        Map.entry(enbas,null),
+        Map.entry(nulpart,null)
+      ), chooser::getSelected)
+    );
+
+
+=======
     manette1.b().onTrue(Commands.either(gratteBaisser, gratteMonte, gratte::getenHaut));
     manette1.leftBumper().toggleOnTrue(cube);
     manette1.rightBumper().toggleOnTrue(cone);
@@ -108,5 +166,6 @@ public RobotContainer() {
       //new PivotBrasRentre(brasTelescopique, pivot).alongWith(new Reculer(basePilotable)),
       //new Gyro(basePilotable)
     //);
+>>>>>>> fc4607f3c6d7203a155182eded4626d37f75de5e
   }
 }

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

@@ -32,7 +32,7 @@ public class Apriltag extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw());
+    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw(), 0);
   }
 
   // Called once the command ends or is interrupted.

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

@@ -32,7 +32,7 @@ public class Cone extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw());
+    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw(), 0);
   }
 
   // Called once the command ends or is interrupted.

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

@@ -32,7 +32,7 @@ public class Cube extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw());
+    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw(), 0);
   }
 
   // Called once the command ends or is interrupted.

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

@@ -28,22 +28,22 @@ public class Gyro extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-  if(basePilotable.getpitch()>4) 
+  if(basePilotable.getpitch()>10) 
    { 
-    basePilotable.drive(0.3*basePilotable.getpitch()/12, 0);
+    basePilotable.drive(0.3*basePilotable.getpitch()/12, 0, 0);
+   }
+   else if(basePilotable.getpitch()<-10) 
+   { 
+    basePilotable.drive(0.3*basePilotable.getpitch()/12, 0, 0);
+    basePilotable.drive(0.3*basePilotable.getpitch()/15, 0, 0);
    }
    else if(basePilotable.getpitch()<-4) 
    { 
-    basePilotable.drive(0.3*basePilotable.getpitch()/12, 0);
-    basePilotable.drive(0.3*basePilotable.getpitch()/15, 0);
-   }
-   else if(basePilotable.getpitch()<-4) 
-   { 
-    basePilotable.drive(0.3*basePilotable.getpitch()/15, 0);
+    basePilotable.drive(0.3*basePilotable.getpitch()/15, 0, 0);
    }
    else
    {
-     basePilotable.drive(0, 0); 
+     basePilotable.drive(0, 0, 0); 
    }
 }
 

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

@@ -27,7 +27,7 @@ public class Reculer extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    basePilotable.drive(SmartDashboard.getNumber("vitesse auto", -0.3), 0);
+    basePilotable.drive(SmartDashboard.getNumber("vitesse auto", -0.3), 0, 0);
   }
 
   // Called once the command ends or is interrupted.

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

@@ -32,7 +32,7 @@ public class Tape extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw());
+    basePilotable.drive(doubleSupplier.getAsDouble(), limelight.getYaw(), 0);
   }
 
   // Called once the command ends or is interrupted.

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

@@ -7,22 +7,25 @@ package frc.robot.subsystems;
 import com.kauailabs.navx.frc.AHRS;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
 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.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
 public class BasePilotable extends SubsystemBase {
-  final CANSparkMax avantd = new CANSparkMax(Constants.avantdroit, MotorType.kBrushless);
+  final CANSparkMax avantdroit = new CANSparkMax(Constants.avantdroit, MotorType.kBrushless);
   final CANSparkMax avantgauche = new CANSparkMax(Constants.avantgauche, MotorType.kBrushless);
   final CANSparkMax arrieredroit = new CANSparkMax(Constants.arrieredroit, MotorType.kBrushless);
   final CANSparkMax arrieregauche = new CANSparkMax(Constants.arrieregauche, MotorType.kBrushless);
-  final MotorControllerGroup droit = new MotorControllerGroup(avantd, arrieredroit);
+  final MotorControllerGroup droit = new MotorControllerGroup(avantdroit, arrieredroit);
   final MotorControllerGroup gauche = new MotorControllerGroup(avantgauche, arrieregauche);
   final DifferentialDrive drive = new DifferentialDrive(gauche, droit);
   
@@ -31,26 +34,35 @@ public class BasePilotable extends SubsystemBase {
   private DoubleSolenoid brakegauche = new DoubleSolenoid(PneumaticsModuleType.CTREPCM,Constants.brakegauche, Constants.brakegauche);
   //gyro
   ShuffleboardTab teb = Shuffleboard.getTab("teb");
+  ShuffleboardLayout layout = Shuffleboard.getTab("teb")
+  .getLayout ("encodeurs base pilotable", BuiltInLayouts.kList)
+  .withSize(2, 2);
   private AHRS gyroscope = new AHRS();public double getangle() {return gyroscope.getAngle();}
    public double getpitch() {
     return gyroscope.getPitch();
   }
 
-  public void drive(double xSpeed, double zRotation){
+  public void drive(double xSpeed, double zRotation, int i){
     drive.arcadeDrive(xSpeed, zRotation);
   }
   public double distance(){
-    return (-avantd.getEncoder().getPosition()
+    teb .add ("distance",0.1);
+    return (-avantdroit.getEncoder().getPosition()
     +avantgauche.getEncoder().getPosition()
     -arrieredroit.getEncoder().getPosition()
     +arrieregauche.getEncoder().getPosition()) / 4;
   }
   public void Reset() {
-    avantd.getEncoder().setPosition(0);
+    avantdroit.getEncoder().setPosition(0);
     avantgauche.getEncoder().setPosition(0);
     arrieredroit.getEncoder().setPosition(0);
     arrieregauche.getEncoder().setPosition(0);
   }
+  public void resetGyro(){
+    {gyroscope.reset();
+    }
+  } 
+
 public void BrakeOuvre(){
   brakedroit.set(Value.kForward);
   brakegauche.set(Value.kForward);
@@ -59,24 +71,13 @@ public void BrakeFerme(){
   brakedroit.set(Value.kReverse);
   brakegauche.set(Value.kReverse);
 }
-public void resetGyro(){
-  {gyroscope.reset();} 
- }
   /** Creates a new BasePilotable. */
   public BasePilotable() {
     droit.setInverted(true);
-    teb .addDouble("distance", this::distance);
   }
 
   @Override
   public void periodic() {
-    //teb .add("encodeuravantdroit",0.1);
-    //teb .add("encodeurarrieregauche",0.1);
-    //teb .add("encodeurarrieredroit",0.1);
-    //teb .add("encodeuravantgauche",0.1);  
-    //teb .add("distance",0.1);
-    //teb .add("brakedroit",0.1);
-    //teb .add("brakegauche", 0.1);
   
   }
 }

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

@@ -13,7 +13,11 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import frc.robot.Constants;
 
 public class Gratte extends SubsystemBase {
+<<<<<<< HEAD
+ ShuffleboardTab teb = Shuffleboard.getTab("teb"); 
+=======
 ShuffleboardTab teb = Shuffleboard.getTab("teb"); 
+>>>>>>> fc4607f3c6d7203a155182eded4626d37f75de5e
 ShuffleboardLayout limitswitchgratte = Shuffleboard.getTab("teb")
 .getLayout("limitswitchsgratte", BuiltInLayouts.kList)
 .withSize(2, 2);

src/main/java/frc/robot/subsystems/bras/BrasTelescopique.java

@@ -18,7 +18,18 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
 public class BrasTelescopique extends SubsystemBase {
+<<<<<<< HEAD
   ShuffleboardTab teb = Shuffleboard.getTab("teb");
+  ShuffleboardLayout layout = Shuffleboard.getTab("teb")
+  .getLayout("layout", BuiltInLayouts.kList)
+  .withSize(2, 2);
+  
+ShuffleboardLayout bras = Shuffleboard.getTab("teb")
+.getLayout("bras", BuiltInLayouts.kList)
+.withSize(2, 2);
+=======
+  ShuffleboardTab teb = Shuffleboard.getTab("teb");  
+>>>>>>> fc4607f3c6d7203a155182eded4626d37f75de5e
   /** Creates a new BrasTelescopique. */
   public BrasTelescopique() {
     teb .add("photocell",0.1);

src/main/java/frc/robot/subsystems/bras/Pivot.java

@@ -14,8 +14,12 @@ import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 public class Pivot extends SubsystemBase {
+<<<<<<< HEAD
+  ShuffleboardTab teb = Shuffleboard.getTab("teb");
+=======
  ShuffleboardTab teb = Shuffleboard.getTab("teb");
   
+>>>>>>> fc4607f3c6d7203a155182eded4626d37f75de5e
   // moteur
   private CANSparkMax pivot = new CANSparkMax(Constants.pivot,MotorType.kBrushless);
   private DigitalInput limitpivot = new DigitalInput(Constants.limitpivot);
@@ -42,5 +46,16 @@ public class Pivot extends SubsystemBase {
   @Override
   public void periodic() {
   }
+<<<<<<< HEAD
+
+  
+ 
+
+  {
+  teb.add ("encodeur pivot",0.1);
+ }
+}
+=======
 }  
 
+>>>>>>> fc4607f3c6d7203a155182eded4626d37f75de5e