pid pour L3

This commit is contained in:
Samuel
2025-10-27 17:45:07 -04:00
parent ad4d20438e
commit 9f64f15ae4
2 changed files with 3 additions and 3 deletions

View File

@@ -33,7 +33,7 @@ public class L3 extends Command {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
output = pidController.calculate(elevateur.position(),20);
output = pidController.calculate(elevateur.position(),-3);
elevateur.vitesse(output);
// if(elevateur.position()<=elevateur.encodeurelevateurL3bas() && elevateur.position()>=elevateur.encodeurelevateurL3haut()){
// elevateur.vitesse(0);

View File

@@ -8,6 +8,8 @@ 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 edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkLowLevel.MotorType;
public class Elevateur extends SubsystemBase {
@@ -38,7 +40,6 @@ public class Elevateur extends SubsystemBase {
}
final SparkMax monte = new SparkMax(22, MotorType.kBrushless);
final DigitalInput limit2 = new DigitalInput(0);
public double position(){
return monte.getEncoder().getPosition();
}
@@ -90,7 +91,6 @@ public class Elevateur extends SubsystemBase {
return distanceDeploiePince.getDouble(0.2);
}
@Override
public void periodic() {
// This method will be called once per scheduler run