test de formule pour b pas concluant(j'ai moins peur)

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.2.1"
+    id "edu.wpi.first.GradleRIO" version "2025.3.2"
 }
 
 java {

src/main/java/frc/robot/commands/Elevateur/L2.java

@@ -3,6 +3,7 @@
 // the WPILib BSD license file in the root directory of this project.
 
 package frc.robot.commands.Elevateur;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Elevateur;
 import frc.robot.subsystems.Pince;
@@ -11,6 +12,8 @@ import frc.robot.subsystems.Pince;
 public class L2 extends Command {
   private Elevateur elevateur;
   private Pince pince;
+    private PIDController pidController;
+  private double output;
   /** Creates a new L2. */
   public L2(Elevateur elevateur,Pince pince) {
     this.elevateur = elevateur;
@@ -21,20 +24,24 @@ public class L2 extends Command {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    pidController = new PIDController(0.025, 0.00125,0.001, 0.01);
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-     if(pince.encodeurpivot()>=15 && pince.encodeurpivot()<=16.5){
+    output = pidController.calculate(pince.encodeurpivot(),15);
-       pince.pivote(0);
+    pince.pivote(output);
-     }
+    //  if(pince.encodeurpivot()>=15 && pince.encodeurpivot()<=16.5){
-     else if(pince.encodeurpivot()>=15){
+    //    pince.pivote(0);
-       pince.pivote(-0.2);
+    //  }
-     }
+    //  else if(pince.encodeurpivot()>=15){
-     else{
+    //    pince.pivote(-0.2);
-       pince.pivote(0.1);
+    //  }
-     }
+    //  else{
+    //    pince.pivote(0.1);
+    //  }
 
   }
 

src/main/java/frc/robot/commands/Elevateur/L3.java

@@ -13,8 +13,10 @@ import frc.robot.subsystems.Pince;
 public class L3 extends Command {
   private Elevateur elevateur;
   private Pince pince;
-  private PIDController pidController;
+  private PIDController pidController1;
+  private PIDController pidController2;
   private double output;
+  private double output2;
   /** Creates a new L2. */
   public L3(Elevateur elevateur,Pince pince) {
     this.elevateur = elevateur;
@@ -26,15 +28,19 @@ public class L3 extends Command {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    pidController = new PIDController(0.03, 0.001, 0.001);
+    pidController1 = new PIDController(0.3, 0.04,0);
-    pidController.reset();
+    pidController2 = new PIDController(0.025, 0.00125,0.001, 0.01);
+    pidController1.reset();
+    pidController2.reset();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    output = pidController.calculate(elevateur.position(),20);
+    output = pidController1.calculate(elevateur.position(),-3);
+    output2 = pidController2.calculate(pince.encodeurpivot(),20);
     elevateur.vitesse(output);
+    pince.pivote(output2);
   // if(elevateur.position()<=elevateur.encodeurelevateurL3bas() && elevateur.position()>=elevateur.encodeurelevateurL3haut()){
   //   elevateur.vitesse(0);
   //   pince.pivote(-0.15);

src/main/java/frc/robot/commands/Elevateur/L4.java

@@ -4,6 +4,7 @@
 
 package frc.robot.commands.Elevateur;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.Elevateur;
 import frc.robot.subsystems.Pince;
@@ -12,6 +13,10 @@ import frc.robot.subsystems.Pince;
 public class L4 extends Command {
   private Elevateur elevateur;
   private Pince pince;
+  private PIDController pidController;
+  private PIDController pidController1;
+  private double output;
+  private double output1;
   /** Creates a new L2. */
   public L4(Elevateur elevateur,Pince pince) {
     this.elevateur = elevateur;
@@ -22,30 +27,39 @@ public class L4 extends Command {
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    pidController = new PIDController(0.3, 0.04,0);
+    pidController1 = new PIDController(0.025, 0.00125,0.001, 0.01);
+    pidController.reset();
+    pidController1.reset();
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(elevateur.position()<=elevateur.encodeurelevateurL4bas() && elevateur.position()>=elevateur.encodeurelevateurL4haut()){
+    output = pidController.calculate(elevateur.position(),-6.3);
-      elevateur.vitesse(0); 
+    elevateur.vitesse(output);
-      pince.pivote(-0.15);
+    output1 = pidController1.calculate(pince.encodeurpivot(),20);
-      if(pince.encodeurpivot()>=20 && pince.encodeurpivot()<=21){
+    pince.pivote(output1);
-        pince.pivote(0);
+    // if(elevateur.position()<=elevateur.encodeurelevateurL4bas() && elevateur.position()>=elevateur.encodeurelevateurL4haut()){
-      }
+    //   elevateur.vitesse(0); 
-      else if(pince.encodeurpivot()>=20){
+    //   pince.pivote(-0.15);
-        pince.pivote(-0.15);
+    //   if(pince.encodeurpivot()>=20 && pince.encodeurpivot()<=21){
-      }
+    //     pince.pivote(0);
-      else{
+    //   }
-        pince.pivote(0.15);
+    //   else if(pince.encodeurpivot()>=20){
-      }
+    //     pince.pivote(-0.15);
-    }
+    //   }
-    else if(elevateur.position()>=elevateur.encodeurelevateurL4bas()){
+    //   else{
-      elevateur.vitesse(-0.7);
+    //     pince.pivote(0.15);
-    }
+    //   }
-    else{
+    // }
-      elevateur.vitesse(.25);
+    // else if(elevateur.position()>=elevateur.encodeurelevateurL4bas()){
-    }
+    //   elevateur.vitesse(-0.7);
+    // }
+    // else{
+    //   elevateur.vitesse(.25);
+    // }
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/Limelight/AprilTag3.java

@@ -47,14 +47,14 @@ public class AprilTag3 extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double a = limelight3.getX();
+    double a = limelight3.getTx();
-   
+  double b = (limelight3.getTA()*100) / (Math.cos(90-limelight3.getTx()));
     if(limelight3.getV() == true){
       drivetrain.setControl(drive.
-      withRotationalRate(a/10).
+      withRotationalRate(a/20).
-      withVelocityX(x.getAsDouble()).
+      withVelocityX(0).
-      withVelocityY(y.getAsDouble()));  
+      withVelocityY( b/50));
-       System.out.println(a/10);
+       System.out.println(b/50);
     }
     else{
       drivetrain.setControl(drive.

src/main/java/frc/robot/commands/Limelight/Forme3.java

@@ -46,7 +46,7 @@ public class Forme3 extends Command {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double a = limelight3.getX();
+    double a = limelight3.getTx();
     if(limelight3.getV() == true){
       drivetrain.setControl(drive.
       withRotationalRate(a/10).

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

@@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 public class Elevateur extends SubsystemBase {
@@ -31,14 +32,13 @@ public class Elevateur extends SubsystemBase {
     teb.add("encodeurelevateursationhaut", -0.4).getEntry();
     private GenericEntry distanceDeploiePince =
     teb.add("encodeurDeploiePince", 0.2).getEntry();
-       
   public Elevateur() {
     teb.addDouble("encodeur elevateur",this::position);
     teb.addBoolean("limit elevateur", this::limit2);
+    teb.addDouble("amperage elevateur", this::amp);
   }
   final SparkMax  monte = new SparkMax(22, MotorType.kBrushless);
   final DigitalInput limit2 = new DigitalInput(0);
-  
   public double position(){ 
     return monte.getEncoder().getPosition();
   }
@@ -89,7 +89,9 @@ public class Elevateur extends SubsystemBase {
   public double distanceDeploiePince(){
     return distanceDeploiePince.getDouble(0.2);
   }
-  
+  public double amp(){
+    return monte.getOutputCurrent();
+  }
   
   @Override
   public void periodic() {

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

@@ -20,8 +20,20 @@ public class Limelight3 extends SubsystemBase {
       PortForwarder.add(port, "limelight.local", port);
     }
   }
-  public double getX(){
+  public double getTx(){
-    return LimelightHelpers.getTX("limelight-balon");
+    NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
+    NetworkTableEntry tx = table.getEntry("tx");
+    return tx.getDouble(0.0);
+  }
+  public double getTA(){
+    NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
+    NetworkTableEntry ta = table.getEntry("ta");
+    return ta.getDouble(0.0);
+  }
+  public double getRx(){
+    NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
+    NetworkTableEntry rx = table.getEntry("rx");
+    return rx.getDouble(0.0);
   }
   public boolean getV(){
     return LimelightHelpers.getTV("limelight-balon");

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

@@ -19,6 +19,7 @@ public class Pince extends SubsystemBase {
     teb.addDouble("encodeur pince", this::encodeurpivot);
     teb.addDouble("amperage corail", this::emperagecoral);
     teb.addDouble("amperage algue", this::emperagealgue);
+    teb.addDouble("amperage pivotis", this::ampPivote);
   }
   final SparkMax coral = new SparkMax(20, MotorType.kBrushless);
   final SparkMax pivoti = new SparkMax(14, MotorType.kBrushless);
@@ -63,7 +64,9 @@ public double emperagealgue(){
   return algue1.getOutputCurrent();
 }
 public boolean x = false;
-
+public double ampPivote(){
+  return pivoti.getOutputCurrent();
+}
   @Override
   public void periodic() {
     // This method will be called once per scheduler run