Merge branch 'main' of https://git.demerso.net/PLS5618/Reefscape-2025
This commit is contained in:
@@ -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.
|
||||||
|
Reference in New Issue
Block a user