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

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

@@ -10,11 +10,16 @@ import com.pathplanner.lib.commands.PathPlannerAuto;
 
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-
+// Manette
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 // Manettes
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -29,7 +34,9 @@ import frc.robot.command.GuiderHaut;
 import frc.robot.command.Lancer;
 import frc.robot.command.LancerNote;
 import frc.robot.command.Lancerampli;
-
+import frc.robot.command.Limelight_tracker;
+import frc.robot.command.PistonFerme;
+import frc.robot.command.Pistongrimpeur;
 // Subsystems
 import frc.robot.subsystem.Accumulateur;
 import frc.robot.subsystem.Balayeuse;
@@ -43,6 +50,7 @@ import frc.robot.subsystem.Pixy;
 
 
 public class RobotContainer {
+  ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
   private final SendableChooser<Command> autoChooser;
   Drive drive = new Drive();
   Accumulateur accumulateur = new Accumulateur();
@@ -50,32 +58,36 @@ public class RobotContainer {
   Grimpeur grimpeur = new Grimpeur();
   Guideur guideur = new Guideur();
   Lanceur lanceur = new Lanceur();
-  Balayer balayer = new Balayer(balayeuse, accumulateur);
-  GuiderHaut guiderHaut = new GuiderHaut(guideur);
-  GuiderBas guiderBas = new GuiderBas(guideur);
   Limelight limelight = new Limelight();
-  Lancer lancer = new Lancer(lanceur,limelight);
-  LancerNote lancernote = new LancerNote(lanceur, accumulateur);
-  Lancerampli lancerampli = new Lancerampli(lanceur,limelight);
+  LED LED = new LED();
+  Pixy pixy = new Pixy();
    CommandJoystick joystick = new CommandJoystick(0);
    CommandXboxController manette = new CommandXboxController(1);
    //command
+   PistonFerme pistonFerme = new PistonFerme(grimpeur);
+   Balayer balayer = new Balayer(balayeuse, accumulateur);
+  GuiderHaut guiderHaut = new GuiderHaut(guideur);
+  Lancer lancer = new Lancer(lanceur,limelight);
+  LancerNote lancernote = new LancerNote(lanceur, accumulateur);
+  Lancerampli lancerampli = new Lancerampli(lanceur,limelight);
    GrimpeurDroit grimpeurDroit = new GrimpeurDroit(grimpeur, manette::getLeftX);
   GrimpeurGauche grimpeurGauche = new GrimpeurGauche(grimpeur, manette::getLeftY);
-  LED LED = new LED();
   AllumeLED allumeLED = new AllumeLED(LED);
-  Pixy pixy = new Pixy();
+  Pistongrimpeur pistongrimpeur = new Pistongrimpeur(grimpeur, LED);
   public RobotContainer() {
+    dashboard.addCamera("limelight", "limelight","limelight.local:5800");
     NamedCommands.registerCommand("balayer",new Balayer(balayeuse, accumulateur));
     NamedCommands.registerCommand("lancer", new LancerNote(lanceur, accumulateur));
+    NamedCommands.registerCommand("piston", new PistonFerme(grimpeur));
     autoChooser = AutoBuilder.buildAutoChooser();
 
     CameraServer.startAutomaticCapture();
-    manette.a().whileTrue(guiderBas);
-    manette.b().whileTrue(guiderHaut);
-    joystick.button(3).toggleOnTrue(balayer);
-    joystick.button(1).whileTrue(lancernote);
-    
+    manette.a().whileTrue(new GuiderBas(guideur));
+    manette.b().whileTrue(new GuiderHaut(guideur));
+    joystick.button(3).toggleOnTrue(new Balayer(balayeuse, accumulateur));
+    joystick.button(1).whileTrue(new LancerNote(lanceur, accumulateur));
+    joystick.button(4).whileTrue(new ParallelCommandGroup(new Lancer(lanceur,limelight),new Limelight_tracker(limelight,drive)));
+    joystick.button(2).whileTrue(new ParallelCommandGroup(new Lancerampli(lanceur,limelight),new Limelight_tracker(limelight,drive),new GuiderHaut(guideur)));
     configureBindings();
      drive.setDefaultCommand(new RunCommand(()->{
      drive.drive(-MathUtil.applyDeadband(joystick.getY(),0.2), MathUtil.applyDeadband(-joystick.getX(),0.2), MathUtil.applyDeadband(-joystick.getZ(), 0.2));
@@ -84,6 +96,9 @@ public class RobotContainer {
       grimpeur.droit(manette.getLeftX());}
       ,grimpeur));
       LED.setDefaultCommand(allumeLED);
+      dashboard.add("autochooser",autoChooser)
+      .withSize(2,1)
+      .withPosition(1,1);
   }
 
   private void configureBindings() {
@@ -92,5 +107,6 @@ public class RobotContainer {
 
   public Command getAutonomousCommand(){
     return autoChooser.getSelected();
+    
   }
 }

src/main/java/frc/robot/command/GrimpeurDroit.java

@@ -56,6 +56,7 @@ public class GrimpeurDroit extends Command {
   @Override
   public void end(boolean interrupted) {
     grimpeur.droit(0);
+    grimpeur.pistonouvre();
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/command/Lancer.java

@@ -19,7 +19,7 @@ public class Lancer extends Command {
     this.limelight = limelight;
     this.lanceur = lanceur;
     // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(lanceur,limelight);
+    addRequirements(lanceur);
   }
 
   // Called when the command is initially scheduled.

src/main/java/frc/robot/command/Lancerampli.java

@@ -17,7 +17,7 @@ public class Lancerampli extends Command {
   private Limelight limelight;
   public Lancerampli(Lanceur lanceur,Limelight limelight) {
     // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(lanceur, limelight);
+    addRequirements(lanceur);
     this.limelight = limelight;
     this.lanceur = lanceur;
   }

src/main/java/frc/robot/command/Limelight_tracker.java

@@ -29,6 +29,7 @@ public class Limelight_tracker extends Command {
 
     if (tracker.getv()){
       drive.drive(0,0, tracker.getx());
+      
       }
       else{
         drive.drive(0, 0, 0);

src/main/java/frc/robot/command/PistonFerme.java

@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.command;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystem.Grimpeur;
+
+public class PistonFerme extends Command {
+  private Grimpeur grimpeur;
+  /** Creates a new PistonFerme. */
+  public PistonFerme(Grimpeur grimpeur) {
+    this.grimpeur = grimpeur;
+    addRequirements(grimpeur);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    grimpeur.pistonferme();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

src/main/java/frc/robot/command/Pistongrimpeur.java

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.command;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystem.Grimpeur;
+import frc.robot.subsystem.LED;
+
+public class Pistongrimpeur extends Command {
+  private LED LED;
+  private Grimpeur grimpeur;
+  /** Creates a new Pistongrimpeur. */
+  public Pistongrimpeur(Grimpeur grimpeur,LED LED) {
+    this.grimpeur = grimpeur;
+    this.LED = LED;
+    addRequirements(grimpeur,LED);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    grimpeur.pistonouvre();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    
+    if(grimpeur.piston()){
+      LED.couleur(0, 0, 255);
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    LED.couleur(0, 0, 0);
+    
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/subsystem/Accumulateur.java

@@ -21,7 +21,9 @@ public class Accumulateur extends SubsystemBase {
   ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
    private GenericEntry vitesse =
       dashboard.add("vitesseacc", 1)
-         .getEntry(); 
+      .withSize(1, 1)
+      .withPosition(4, 3)
+      .getEntry(); 
   final WPI_TalonSRX Moteuracc2 = new WPI_TalonSRX(Constants.Moteuracc2);
   final WPI_TalonSRX Moteuracc = new WPI_TalonSRX(Constants.Moteuracc);
   final DigitalInput photocellacc = new DigitalInput(Constants.photocellacc);

src/main/java/frc/robot/subsystem/Drive.java

@@ -32,6 +32,11 @@ public class Drive extends SubsystemBase {
   }
   
   public Drive() {
+     try {
+      this.swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(2);
+    } catch (IOException e) {
+      e.printStackTrace();
+    }
     AutoBuilder.configureHolonomic(swerveDrive::getPose, swerveDrive::resetOdometry, swerveDrive::getRobotVelocity, swerveDrive::setChassisSpeeds, new HolonomicPathFollowerConfig(
       new PIDConstants(5,0,0), new PIDConstants(5,0,0), 4.5,
       0.275,
@@ -43,11 +48,6 @@ public class Drive extends SubsystemBase {
         }
         return false;
          }, this);
-     try {
-      this.swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(2);
-    } catch (IOException e) {
-      e.printStackTrace();
-    }
   }
 
   public SwerveModulePosition[] distance(){

src/main/java/frc/robot/subsystem/Grimpeur.java

@@ -12,8 +12,11 @@ import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.Solenoid;
 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.shuffleboard.BuiltInLayouts;
 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
@@ -23,30 +26,24 @@ public class Grimpeur extends SubsystemBase {
   // moteur
   
   ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
+  ShuffleboardLayout layout = Shuffleboard.getTab("dashboard")
+  .getLayout("grimpeur", BuiltInLayouts.kList)
+  .withSize(2,4)
+  .withPosition(2,1);
   final CANSparkMax grimpeurd = new CANSparkMax(Constants.grimpeurd,MotorType.kBrushless);
   final CANSparkMax grimpeurg = new CANSparkMax(Constants.grimpeurg,MotorType.kBrushless); 
   // limit switch
   final DigitalInput limitdroite = new DigitalInput(Constants.limithaut);
   final DigitalInput limitgauche = new DigitalInput(Constants.limitbas);
-  final DoubleSolenoid pistondroite= new DoubleSolenoid(PneumaticsModuleType.CTREPCM, Constants.pistondroiteouvre, Constants.pistondroiteouvre);
-  final DoubleSolenoid pistondgauche= new DoubleSolenoid(PneumaticsModuleType.CTREPCM, Constants.pistondgaucheouvre, Constants.pistondroiteouvre);
+  final Solenoid pistondroite= new Solenoid(PneumaticsModuleType.CTREPCM, Constants.pistondroiteouvre);
+  final Solenoid pistondgauche = new Solenoid(PneumaticsModuleType.CTREPCM, Constants.pistondgaucheouvre);
   //fonction
   public Grimpeur() {
-    dashboard.add("limitgrimpeurd", droite())
-    .withSize(1, 1)
-    .withPosition(1, 5);
-    dashboard.add("limitgrimpeurd", droite())
-    .withSize(1, 1)
-    .withPosition(1, 4);
-    dashboard.add("grimpeurencodeurd", encoderd())
-    .withSize(1, 1)
-    .withPosition(1, 3);
-    dashboard.add("grimpeurencodeurg", encoderg())
-    .withSize(1, 1)
-    .withPosition(1, 2);
-    dashboard.add("pitchgyrogrimpeur", getpitch())
-    .withSize(1, 1)
-    .withPosition(1, 1);
+    layout.addBoolean("limitgrimpeurd", limitdroite::get);
+    layout.addBoolean("limitgrimpeurg", limitdroite::get);
+    layout.add("grimpeurencodeurd", encoderd());
+    layout.add("grimpeurencodeurg", encoderg());
+    layout.add("pitchgyrogrimpeur", getpitch());
   }
 public void droit(double vitesse){
   grimpeurd.set(vitesse);
@@ -76,13 +73,16 @@ public AHRS gyroscope = new AHRS();
   public double getpitch(){
     return gyroscope.getPitch();
   }
-  public void pistonouvre(){
-    pistondroite.set(Value.kForward);
-    pistondgauche.set(Value.kForward);
-  }
   public void pistonferme(){
-    pistondroite.set(Value.kReverse);
-    pistondgauche.set(Value.kReverse);
+     pistondroite.set(true);
+     pistondgauche.set(true);
+  }
+  public void pistonouvre(){
+    pistondgauche.set(false);
+    pistondroite.set(false);
+  }
+  public boolean piston(){
+    return pistondgauche.get();
   }
   @Override
   public void periodic() {

src/main/java/frc/robot/subsystem/Guideur.java

@@ -8,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 import edu.wpi.first.wpilibj.DigitalInput;
 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;
@@ -16,12 +17,16 @@ public class Guideur extends SubsystemBase {
   /** Creates a new Guideur. */
   
     ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
+    ShuffleboardLayout layout = Shuffleboard.getTab("dashboard")
+    .getLayout("grimpeur")
+    .withSize(2, 2)
+    .withPosition(1, 3);
     final WPI_TalonSRX guideur = new WPI_TalonSRX(Constants.guideur);
     final DigitalInput guideurhaut = new DigitalInput(Constants.guideurhaut);
     final DigitalInput guideurbas = new DigitalInput(Constants.guideurbas);
       public Guideur() {
-        dashboard.add("limitguideurhaut", haut());
-        dashboard.add("limitguideurbas", bas());
+        dashboard.addBoolean("limitguideurhaut", guideurhaut::get);
+        dashboard.addBoolean("limitguideurbas", guideurbas::get);        
       }
       public void guider(double vitesse){
         guideur.set(vitesse);

src/main/java/frc/robot/subsystem/LED.java

@@ -12,14 +12,19 @@ public class LED extends SubsystemBase {
   /** Creates a new LED. */
   public LED() {}
    AddressableLED led = new AddressableLED(9);
-   AddressableLEDBuffer ledBuffer = new AddressableLEDBuffer(60);
+   AddressableLEDBuffer ledBuffer = new AddressableLEDBuffer(150);
    public void led(){
     led.setData(ledBuffer);
-    led.start();
-   }
-   public void couleur(int R, int G, int B){
-    ledBuffer.setRGB(0, R, G, B);
+    led.start();}
+   
+   public void couleur(int R, int G,int B){
+    for (int i = 0; i < ledBuffer.getLength(); i++) {
+    // Sets the specified LED to the RGB values for red
+    ledBuffer.setRGB(i, 255, 0, 0);}
+   
    }
+  
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run

src/main/java/frc/robot/subsystem/Lanceur.java

@@ -18,9 +18,13 @@ public class Lanceur extends SubsystemBase {
   ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
    private GenericEntry vitesse =
       dashboard.add("vitesse", 1)
+      .withSize(1, 1)
+      .withPosition(3, 3)
          .getEntry();
          private GenericEntry vitesseamp =
             dashboard.add("vitesseamp", 1)
+            .withSize(1, 1)
+            .withPosition(3, 4)
                .getEntry();
   public Lanceur() {