Merge branch 'master' of https://git.demerso.net/pls5618/2024/betabot-2024

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

@@ -3,36 +3,80 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package frc.robot;
+import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.XboxController;
+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.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.CommandJoystick;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.commands.Avancer;
+import frc.robot.commands.Force1;
+import frc.robot.commands.Force2;
+import frc.robot.commands.Force3;
+import frc.robot.commands.Force4;
+import frc.robot.commands.Force5;
+import frc.robot.commands.Force6;
+import frc.robot.commands.Force7;
+import frc.robot.commands.Reculer;
+import frc.robot.subsystems.Accumulateur;
 import frc.robot.subsystems.Drive;
+import frc.robot.subsystems.Lanceur;
 
 public class RobotContainer {
+  Lanceur lanceur = new Lanceur();
+  Accumulateur accumulateur = new Accumulateur();
+  
+  
+
+  ShuffleboardTab dashboard = Shuffleboard.getTab("Dashboard");
+  ShuffleboardLayout forces = Shuffleboard.getTab("Dashboard")
+  .getLayout("limitswitchsgratte", BuiltInLayouts.kList)
+  .withSize(3, 7);
+  GenericEntry force1 = forces.add("Force1", 0).getEntry();
+  GenericEntry force2 = forces.add("Force2", 0).getEntry();
+  GenericEntry force3 = forces.add("Force3", 0).getEntry();
+  GenericEntry force4 = forces.add("Force4", 0).getEntry();
+  GenericEntry force5 = forces.add("Force5", 0).getEntry();
+  GenericEntry force6 = forces.add("Force6", 0).getEntry();
+  GenericEntry force7 = forces.add("Force7", 0).getEntry();
   CommandXboxController manette = new CommandXboxController(0);
-  Joystick joystick1 = new Joystick(0);
+  CommandJoystick joystick1 = new CommandJoystick(0);
   Drive drive = new Drive();
   public RobotContainer() {
+    
     configureBindings();
-   // drive.setDefaultCommand(new RunCommand(()->{
-     // drive.drive(manette.getLeftX(), manette.getLeftY(), manette.getRightX());
-    //},drive));
     drive.setDefaultCommand(new RunCommand(()->{
      drive.drive(joystick1.getX(), -joystick1.getY(), -joystick1.getZ());
     },drive));
   }
   
   private void configureBindings() {
-    JoystickButton button =new JoystickButton(joystick1, 1);
+    Force1 Force1 = new Force1(lanceur, accumulateur, null);
+  Force2 Force2 = new Force2(lanceur, accumulateur, null);
+  Force3 Force3 = new Force3(lanceur, null, accumulateur);
+  Force4 Force4 = new Force4(lanceur, null, accumulateur);
+  Force5 Force5 = new Force5(lanceur, null, accumulateur);
+  Force6 Force6 = new Force6(lanceur, null, accumulateur);
+  Force7 Force7 = new Force7(lanceur, null, accumulateur);
+    joystick1.button(7).onTrue(Force1);
+    joystick1.button(8).onTrue(Force2);
+    joystick1.button(9).onTrue(Force3);
+    joystick1.button(10).onTrue(Force4);
+    joystick1.button(11).onTrue(Force5); 
+    joystick1.button(12).onTrue(Force6);
+    joystick1.button(3).onTrue(Force7);
+
     
   }
  
   public Command getAutonomousCommand() {
-    return new SequentialCommandGroup(null);
+    return new SequentialCommandGroup(new Avancer(drive), new Force7(lanceur, force7, accumulateur)
+    , new Force1(lanceur, accumulateur, force1), new Reculer(drive));
   }
 }

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

@@ -0,0 +1,40 @@
+// 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.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Drive;
+
+public class Avancer extends CommandBase {
+  private Drive drive;
+  /** Creates a new Avancer. */
+  public Avancer(Drive drive) {
+    this.drive = drive;
+    addRequirements(drive);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+   drive.drive(0.5, 0.2, 0);
+  }
+
+  // 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 drive.distance()[0].distanceMeters>1;
+  }
+}

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

@@ -3,20 +3,25 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package frc.robot.commands;
-import frc.robot.subsystems.Accumulateur;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+
+import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Accumulateur;
 import frc.robot.subsystems.Lanceur;
 
 public class Force1 extends CommandBase {
-
   private Lanceur lanceur;
   private Accumulateur accumulateur;
+  GenericEntry force1;
   
-  public void Lancer(Lanceur lanceur) {
+  /** Creates a new Force1. */
+  public Force1(Lanceur lanceur,Accumulateur accumulateur, GenericEntry force1) {
     this.lanceur = lanceur;
     this.accumulateur = accumulateur;
     addRequirements(lanceur, accumulateur);
+    this.force1 = force1; 
+    // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
@@ -28,32 +33,26 @@ public class Force1 extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double vitesse = SmartDashboard.getNumber("Force Lanceur", 100);
-      lanceur.lancer(200);
+    lanceur.lancer(force1.getDouble(100));
+    double vitesse = (100);
     if (lanceur.vitesse() > vitesse ){
-       if(accumulateur.tourneavant() < 1024){
-        accumulateur.Deaccumuler();
-      }
-      if(accumulateur.tournearriere()<-256){
-        accumulateur.reaccumuler();
-         }
+      accumulateur.tourneavant();
       accumulateur.tournearriere();
       } else {
         accumulateur.stop();
       }
-      
   }
   
+
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    lanceur.stop();
-    accumulateur.stop();
+    lanceur.lancer(0);
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return lanceur.distance()>1;
+    return false;
   }
 }

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

@@ -2,25 +2,24 @@
 // 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.
 
-// 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.commands;
-import frc.robot.subsystems.Accumulateur;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Accumulateur;
 import frc.robot.subsystems.Lanceur;
 
 public class Force2 extends CommandBase {
-
   private Lanceur lanceur;
   private Accumulateur accumulateur;
-
-  public void Lancer(Lanceur lanceur) {
+  GenericEntry force2;
+  /** Creates a new Force1. */
+  public Force2(Lanceur lanceur,Accumulateur accumulateur, GenericEntry force2) {
     this.lanceur = lanceur;
     this.accumulateur = accumulateur;
     addRequirements(lanceur, accumulateur);
+    this.force2 = force2;
+    // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
@@ -32,27 +31,25 @@ public class Force2 extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double vitesse = SmartDashboard.getNumber("Force Lanceur", 200);
-      lanceur.lancer(200);
+    double vitesse = (200);
+    lanceur.lancer(force2.getDouble(200));
     if (lanceur.vitesse() > vitesse ){
       accumulateur.tourneavant();
       accumulateur.tournearriere();
       } else {
         accumulateur.stop();
       }
-      
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    lanceur.stop();
-    accumulateur.stop();
+    lanceur.lancer(0);
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return lanceur.distance()>1;
+    return false;
   }
 }

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

@@ -8,7 +8,9 @@
 
 package frc.robot.commands;
 import frc.robot.subsystems.Accumulateur;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+
+import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.Lanceur;
 
@@ -16,11 +18,15 @@ public class Force3 extends CommandBase {
 
   private Lanceur lanceur;
   private Accumulateur accumulateur;
+  GenericEntry force3;
   
-  public void Lancer(Lanceur lanceur) {
+  /** Creates a new Force1. */
+  public Force3(Lanceur lanceur,GenericEntry force3, Accumulateur accumulateur) {
     this.lanceur = lanceur;
-    this.accumulateur = accumulateur;
     addRequirements(lanceur, accumulateur);
+    this.force3 = force3; 
+    this.accumulateur = accumulateur;
+    // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
@@ -32,27 +38,18 @@ public class Force3 extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double vitesse = SmartDashboard.getNumber("Force Lanceur", 300);
-      lanceur.lancer(200);
+
+    double vitesse = (300);
+    lanceur.lancer(force3.getDouble(300));
       if (lanceur.vitesse() > vitesse ){
         accumulateur.tourneavant();
         accumulateur.tournearriere();
         } else {
           accumulateur.stop();
         }
-      
     }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-    lanceur.stop();
-    accumulateur.stop();
-    }
-
-  // Returns true when the command should end.
     @Override
     public boolean isFinished() {
-    return lanceur.distance()>1;
+      return false;
     }
   }

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

@@ -4,7 +4,9 @@
 
 package frc.robot.commands;
 import frc.robot.subsystems.Accumulateur;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+
+import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.Lanceur;
 
@@ -12,11 +14,15 @@ public class Force4 extends CommandBase {
 
   private Lanceur lanceur;
   private Accumulateur accumulateur;
+  GenericEntry force4;
   
-  public void Lancer(Lanceur lanceur) {
+  /** Creates a new Force1. */
+  public Force4(Lanceur lanceur,GenericEntry force4, Accumulateur accumulateur) {
     this.lanceur = lanceur;
-    this.accumulateur = accumulateur;
     addRequirements(lanceur, accumulateur);
+    this.force4 = force4; 
+    this.accumulateur = accumulateur;
+    // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
@@ -28,8 +34,8 @@ public class Force4 extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double vitesse = SmartDashboard.getNumber("Force Lanceur", 400);
-      lanceur.lancer(200);
+    double vitesse = (400);
+      lanceur.lancer(force4.getDouble(400));
     if (lanceur.vitesse() > vitesse ){
       accumulateur.tourneavant();
       accumulateur.tournearriere();
@@ -37,6 +43,7 @@ public class Force4 extends CommandBase {
         accumulateur.stop();
       }
       
+    lanceur.lancer(force4.getDouble(0));
   }
 
   // Called once the command ends or is interrupted.

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

@@ -4,7 +4,9 @@
 
 package frc.robot.commands;
 import frc.robot.subsystems.Accumulateur;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+
+import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.Lanceur;
 
@@ -12,11 +14,15 @@ public class Force5 extends CommandBase {
 
   private Lanceur lanceur;
   private Accumulateur accumulateur;
+  GenericEntry force5;
   
-  public void Lancer(Lanceur lanceur) {
+  /** Creates a new Force1. */
+  public Force5(Lanceur lanceur,GenericEntry force5, Accumulateur accumulateur) {
     this.lanceur = lanceur;
-    this.accumulateur = accumulateur;
     addRequirements(lanceur, accumulateur);
+    this.force5 = force5; 
+    this.accumulateur = accumulateur;
+    // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
@@ -28,8 +34,8 @@ public class Force5 extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double vitesse = SmartDashboard.getNumber("Force Lanceur", 500);
-      lanceur.lancer(200);
+    double vitesse = (500);
+      lanceur.lancer(force5.getDouble(500));
     if (lanceur.vitesse() > vitesse ){
       accumulateur.tourneavant();
       accumulateur.tournearriere();
@@ -37,6 +43,7 @@ public class Force5 extends CommandBase {
         accumulateur.stop();
       }
       
+    lanceur.lancer(force5.getDouble(0));
   }
 
   // Called once the command ends or is interrupted.

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

@@ -4,7 +4,7 @@
 
 package frc.robot.commands;
 import frc.robot.subsystems.Accumulateur;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.Lanceur;
 
@@ -12,11 +12,16 @@ public class Force6 extends CommandBase {
 
   private Lanceur lanceur;
   private Accumulateur accumulateur;
+  GenericEntry force6;
   
-  public void Lancer(Lanceur lanceur) {
+  
+  /** Creates a new Force1. */
+  public Force6(Lanceur lanceur, GenericEntry force6, Accumulateur accumulateur) {
     this.lanceur = lanceur;
-    this.accumulateur = accumulateur;
     addRequirements(lanceur, accumulateur);
+    this.force6 = force6;
+    this.accumulateur = accumulateur;
+    // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
@@ -28,8 +33,8 @@ public class Force6 extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double vitesse = SmartDashboard.getNumber("Force Lanceur", 600);
-      lanceur.lancer(200);
+    double vitesse = (600);
+      lanceur.lancer(force6.getDouble(600));
     if (lanceur.vitesse() > vitesse ){
       accumulateur.tourneavant();
       accumulateur.tournearriere();
@@ -37,6 +42,7 @@ public class Force6 extends CommandBase {
         accumulateur.stop();
       }
       
+    lanceur.lancer(force6.getDouble(0));
   }
 
   // Called once the command ends or is interrupted.

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

@@ -4,7 +4,7 @@
 
 package frc.robot.commands;
 import frc.robot.subsystems.Accumulateur;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.subsystems.Lanceur;
 
@@ -12,11 +12,15 @@ public class Force7 extends CommandBase {
 
   private Lanceur lanceur;
   private Accumulateur accumulateur;
+  GenericEntry force7;
    
-  public void Lancer(Lanceur lanceur) {
+  /** Creates a new Force1. */
+  public Force7(Lanceur lanceur,GenericEntry force7, Accumulateur accumulateur) {
     this.lanceur = lanceur;
-    this.accumulateur = accumulateur;
     addRequirements(lanceur, accumulateur);
+    this.force7 = force7; 
+    this.accumulateur = accumulateur;
+    // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
@@ -28,8 +32,8 @@ public class Force7 extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double vitesse = SmartDashboard.getNumber("Force Lanceur", 700);
-      lanceur.lancer(200);
+    double vitesse = (700);
+      lanceur.lancer(force7.getDouble(700));
     if (lanceur.vitesse() > vitesse ){
       accumulateur.tourneavant();
       accumulateur.tournearriere();
@@ -37,6 +41,7 @@ public class Force7 extends CommandBase {
         accumulateur.stop();
       }
       
+    lanceur.lancer(force7.getDouble(0));
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/Reculer.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.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Drive;
+
+public class Reculer extends CommandBase {
+  private Drive drive;
+  /** Creates a new Reculer. */
+  public Reculer(Drive drive) {
+    this.drive = drive;
+    addRequirements(drive);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    drive.drive(-0.5, -0.2, 0); 
+  }
+
+  // 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 drive.distance()[0].distanceMeters>1;
+  }
+}

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

@@ -7,7 +7,7 @@ package frc.robot.subsystems;
 
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
-import edu.wpi.first.wpilibj.DigitalInput;
+
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 

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

@@ -8,6 +8,7 @@ import java.io.File;
 import java.io.IOException;
 
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import swervelib.SwerveDrive;
@@ -30,7 +31,9 @@ public class Drive extends SubsystemBase {
       e.printStackTrace();
     }
   }
-
+   public SwerveModulePosition[] distance(){
+    return swerveDrive.getModulePositions();
+  }
   @Override
   public void periodic() {
     // This method will be called once per scheduler run

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

@@ -6,14 +6,14 @@ package frc.robot.subsystems;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
 public class Lanceur extends SubsystemBase {
   
-  final CANSparkMax lanceur = new CANSparkMax(Constants.lanceur, MotorType.kBrushless);
-  
   public Lanceur(){}
+  final CANSparkMax lanceur = new CANSparkMax(Constants.lanceur, MotorType.kBrushless); 
 
   public void lancer(double vitesse){
     lanceur.set(vitesse);