pid fonctionne mais pas de pid
This commit is contained in:
@@ -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.
|
||||
|
Reference in New Issue
Block a user