From 837435643bedbc1c6f422f42ac89e06619d24133 Mon Sep 17 00:00:00 2001 From: EdwardFaucher Date: Mon, 20 Oct 2025 18:42:24 -0400 Subject: [PATCH] pid fonctionne mais pas de pid --- .../java/frc/robot/commands/Elevateur/L3.java | 49 +++++++++++-------- 1 file changed, 28 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/commands/Elevateur/L3.java b/src/main/java/frc/robot/commands/Elevateur/L3.java index cb940c0..8011b68 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L3.java +++ b/src/main/java/frc/robot/commands/Elevateur/L3.java @@ -5,7 +5,7 @@ package frc.robot.commands.Elevateur; import edu.wpi.first.wpilibj2.command.Command; - +import edu.wpi.first.math.controller.PIDController; import frc.robot.subsystems.Elevateur; import frc.robot.subsystems.Pince; @@ -13,6 +13,8 @@ import frc.robot.subsystems.Pince; public class L3 extends Command { private Elevateur elevateur; private Pince pince; + private PIDController pidController; + private double output; /** Creates a new L2. */ public L3(Elevateur elevateur,Pince pince) { this.elevateur = elevateur; @@ -23,30 +25,35 @@ public class L3 extends Command { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + pidController = new PIDController(0.03, 0.001, 0.001); + pidController.reset(); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(elevateur.position()<=elevateur.encodeurelevateurL3bas() && elevateur.position()>=elevateur.encodeurelevateurL3haut()){ - elevateur.vitesse(0); - pince.pivote(-0.15); - if(pince.encodeurpivot()>=20 && pince.encodeurpivot()<=21){ - pince.pivote(0); - } - else if(pince.encodeurpivot()>=20){ - pince.pivote(-0.15); - } - else{ - pince.pivote(0.15); - } - } - else if(elevateur.position()>=elevateur.encodeurelevateurL3bas()){ - elevateur.vitesse(-0.7); - } - else{ - elevateur.vitesse(0.25); - } + output = pidController.calculate(elevateur.position(),20); + elevateur.vitesse(output); + // if(elevateur.position()<=elevateur.encodeurelevateurL3bas() && elevateur.position()>=elevateur.encodeurelevateurL3haut()){ + // elevateur.vitesse(0); + // pince.pivote(-0.15); + // if(pince.encodeurpivot()>=20 && pince.encodeurpivot()<=21){ + // pince.pivote(0); + // } + // else if(pince.encodeurpivot()>=20){ + // pince.pivote(-0.15); + // } + // else{ + // pince.pivote(0.15); + // } + // } + // else if(elevateur.position()>=elevateur.encodeurelevateurL3bas()){ + // elevateur.vitesse(-0.7); + // } + // else{ + // elevateur.vitesse(0.25); + // } } // Called once the command ends or is interrupted.