This commit is contained in:
Samuel
2025-10-20 18:39:12 -04:00

View File

@@ -5,7 +5,7 @@
package frc.robot.commands.Elevateur; package frc.robot.commands.Elevateur;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.math.controller.PIDController;
import frc.robot.subsystems.Elevateur; import frc.robot.subsystems.Elevateur;
import frc.robot.subsystems.Pince; import frc.robot.subsystems.Pince;
@@ -13,6 +13,8 @@ import frc.robot.subsystems.Pince;
public class L3 extends Command { public class L3 extends Command {
private Elevateur elevateur; private Elevateur elevateur;
private Pince pince; private Pince pince;
private PIDController pidController;
private double output;
/** Creates a new L2. */ /** Creates a new L2. */
public L3(Elevateur elevateur,Pince pince) { public L3(Elevateur elevateur,Pince pince) {
this.elevateur = elevateur; this.elevateur = elevateur;
@@ -23,30 +25,35 @@ public class L3 extends Command {
// Called when the command is initially scheduled. // Called when the command is initially scheduled.
@Override @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. // Called every time the scheduler runs while the command is scheduled.
@Override @Override
public void execute() { public void execute() {
if(elevateur.position()<=elevateur.encodeurelevateurL3bas() && elevateur.position()>=elevateur.encodeurelevateurL3haut()){ output = pidController.calculate(elevateur.position(),20);
elevateur.vitesse(0); elevateur.vitesse(output);
pince.pivote(-0.15); // if(elevateur.position()<=elevateur.encodeurelevateurL3bas() && elevateur.position()>=elevateur.encodeurelevateurL3haut()){
if(pince.encodeurpivot()>=20 && pince.encodeurpivot()<=21){ // elevateur.vitesse(0);
pince.pivote(0); // pince.pivote(-0.15);
} // if(pince.encodeurpivot()>=20 && pince.encodeurpivot()<=21){
else if(pince.encodeurpivot()>=20){ // pince.pivote(0);
pince.pivote(-0.15); // }
} // else if(pince.encodeurpivot()>=20){
else{ // pince.pivote(-0.15);
pince.pivote(0.15); // }
} // else{
} // pince.pivote(0.15);
else if(elevateur.position()>=elevateur.encodeurelevateurL3bas()){ // }
elevateur.vitesse(-0.7); // }
} // else if(elevateur.position()>=elevateur.encodeurelevateurL3bas()){
else{ // elevateur.vitesse(-0.7);
elevateur.vitesse(0.25); // }
} // else{
// elevateur.vitesse(0.25);
// }
} }
// Called once the command ends or is interrupted. // Called once the command ends or is interrupted.