Commit

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

@@ -18,7 +18,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
 // Manettes
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 // Commands
 import frc.robot.command.AllumeLED;
 import frc.robot.command.Balayer;
@@ -78,7 +78,7 @@ public class RobotContainer {
     autoChooser = AutoBuilder.buildAutoChooser();
 
     CameraServer.startAutomaticCapture();
-    manette.a().whileTrue(new GuiderBas(guideur));
+    /*manette.a().whileTrue(new GuiderBas(guideur));
     manette.b().whileTrue(new GuiderHaut(guideur));
     manette.x().whileTrue(new PistonFerme(grimpeur));
     manette.y().whileTrue(new PistonOuvre(grimpeur));
@@ -86,11 +86,11 @@ public class RobotContainer {
     joystick.button(1).whileTrue(new LancerNote(lanceur, accumulateur));
     joystick.button(5).whileTrue(new LancerAmp(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)));
+    joystick.button(2).whileTrue(new ParallelCommandGroup(new Lancerampli(lanceur,limelight),new Limelight_tracker(limelight,drive),new GuiderHaut(guideur)));*/
     
     // deplacement
     configureBindings();
-     drive.setDefaultCommand(new RunCommand(()->{
+    /*  drive.setDefaultCommand(new RunCommand(()->{
      drive.drive(MathUtil.applyDeadband(joystick.getY(),0.2), MathUtil.applyDeadband(joystick.getX(),0.2), MathUtil.applyDeadband(joystick.getZ(), 0.2));
     },drive));
 
@@ -103,7 +103,24 @@ public class RobotContainer {
       LED.setDefaultCommand(allumeLED);
       dashboard.add("autochooser",autoChooser)
       .withSize(2,1)
-      .withPosition(1,1);
+      .withPosition(1,1);*/
+      manette
+        .a()
+        .and(manette.rightBumper())
+        .whileTrue(lanceur.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    manette
+        .b()
+        .and(manette.rightBumper())
+        .whileTrue(lanceur.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    manette
+        .x()
+        .and(manette.rightBumper())
+        .whileTrue(lanceur.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    manette
+        .y()
+        .and(manette.rightBumper())
+        .whileTrue(lanceur.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+
   }
 
   private void configureBindings() {}

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

@@ -7,11 +7,22 @@ package frc.robot.subsystem;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
-
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.MutableMeasure.mutable;
 public class Lanceur extends SubsystemBase {
   /** Creates a new Lanceur. */
   ShuffleboardTab dashboard = Shuffleboard.getTab("dashboard");
@@ -32,6 +43,11 @@ public class Lanceur extends SubsystemBase {
   final CANSparkMax lanceur2 = new CANSparkMax(Constants.lancer2, MotorType.kBrushless);
   final CANSparkMax lanceur3 = new CANSparkMax(Constants.lancer3, MotorType.kBrushless);
   final CANSparkMax lanceur4 = new CANSparkMax(Constants.lancer4, MotorType.kBrushless);
+  private final MutableMeasure<Voltage> appliedVoltage = mutable(Volts.of(0));
+  // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation.
+  private final MutableMeasure<Angle> angle = mutable(Rotations.of(0));
+  // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation.
+  private final MutableMeasure<Velocity<Angle>> velocity = mutable(RotationsPerSecond.of(0));
   public void pidspeaker(){
     lanceur1.getPIDController().setP(0.5);
     lanceur1.getPIDController().setI(0.5);
@@ -46,6 +62,30 @@ public class Lanceur extends SubsystemBase {
     lanceur4.getPIDController().setI(0.5);
     lanceur4.getPIDController().setD(0.5);
   }
+   private final SysIdRoutine sysIdRoutine =
+      new SysIdRoutine(
+          // Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage.
+          new SysIdRoutine.Config(),
+          new SysIdRoutine.Mechanism(
+              // Tell SysId how to plumb the driving voltage to the motor(s).
+              (Measure<Voltage> volts) -> {
+                lanceur1.setVoltage(volts.in(Volts));
+              },
+              // Tell SysId how to record a frame of data for each motor on the mechanism being
+              // characterized.
+              log -> {
+                // Record a frame for the shooter motor.
+                log.motor("shooter-wheel")
+                    .voltage(
+                        appliedVoltage.mut_replace(
+                            lanceur1.get() * RobotController.getBatteryVoltage(), Volts))
+                    .angularPosition(angle.mut_replace(lanceur1.getEncoder().getPosition(), Rotations))
+                    .angularVelocity(
+                        velocity.mut_replace(lanceur1.getEncoder().getVelocity(), RotationsPerSecond));
+              },
+              // Tell SysId to make generated commands require this subsystem, suffix test state in
+              // WPILog with this subsystem's name ("shooter")
+              this));
   public void lancer(double vitesse){
     lanceur1.set(-vitesse);
     lanceur2.set(-vitesse);
@@ -64,7 +104,18 @@ public class Lanceur extends SubsystemBase {
     public void lanceur(){
    
   }
+  public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+    return sysIdRoutine.quasistatic(direction);
+  }
 
+  /**
+   * Returns a command that will execute a dynamic test in the given direction.
+   *
+   * @param direction The direction (forward or reverse) to run the test in
+   */
+  public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+    return sysIdRoutine.dynamic(direction);
+  }
   @Override
   public void periodic() {}
     // This method will be called once per scheduler run