PLS5618/Reefscape-2025
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025

Browse Source
This commit is contained in:
Samuel
2025-10-20 18:39:12 -04:00
parent 0bf575ff37 837435643b
commit ad4d20438e
1 changed files with 28 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
src/main/java/frc/robot/commands/Elevateur/L3.java
View File

@@ -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.