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

pid d`édouard

Browse Source
This commit is contained in:
EdwardFaucher
2025-10-29 18:34:30 -04:00
parent 837435643b
commit 3715451972
8 changed files with 84 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
src/main/java/frc/robot/commands/Elevateur/L2.java
View File

@@ -3,6 +3,7 @@
// the WPILib BSD license file in the root directory of this project. // the WPILib BSD license file in the root directory of this project.
package frc.robot.commands.Elevateur; package frc.robot.commands.Elevateur;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Elevateur; import frc.robot.subsystems.Elevateur;
import frc.robot.subsystems.Pince; import frc.robot.subsystems.Pince;
@@ -11,6 +12,8 @@ import frc.robot.subsystems.Pince;
public class L2 extends Command { public class L2 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 L2(Elevateur elevateur,Pince pince) { public L2(Elevateur elevateur,Pince pince) {
this.elevateur = elevateur; this.elevateur = elevateur;
@@ -21,20 +24,24 @@ public class L2 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.025, 0.00125,0.001, 0.01);
}
// 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(pince.encodeurpivot()>=15 && pince.encodeurpivot()<=16.5){ output = pidController.calculate(pince.encodeurpivot(),15);
pince.pivote(0); pince.pivote(output);
} // if(pince.encodeurpivot()>=15 && pince.encodeurpivot()<=16.5){
else if(pince.encodeurpivot()>=15){ // pince.pivote(0);
pince.pivote(-0.2); // }
} // else if(pince.encodeurpivot()>=15){
else{ // pince.pivote(-0.2);
pince.pivote(0.1); // }
} // else{
// pince.pivote(0.1);
// }
} }

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

@@ -13,8 +13,10 @@ 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 PIDController pidController1;
private PIDController pidController2;
private double output; private double output;
private double output2;
/** 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;
@@ -26,15 +28,19 @@ 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); pidController1 = new PIDController(0.3, 0.04,0);
pidController.reset(); pidController2 = new PIDController(0.025, 0.00125,0.001, 0.01);
pidController1.reset();
pidController2.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() {
output = pidController.calculate(elevateur.position(),20); output = pidController1.calculate(elevateur.position(),-3);
output2 = pidController2.calculate(pince.encodeurpivot(),20);
elevateur.vitesse(output); elevateur.vitesse(output);
pince.pivote(output2);
// if(elevateur.position()<=elevateur.encodeurelevateurL3bas() && elevateur.position()>=elevateur.encodeurelevateurL3haut()){ // if(elevateur.position()<=elevateur.encodeurelevateurL3bas() && elevateur.position()>=elevateur.encodeurelevateurL3haut()){
// elevateur.vitesse(0); // elevateur.vitesse(0);
// pince.pivote(-0.15); // pince.pivote(-0.15);

54
src/main/java/frc/robot/commands/Elevateur/L4.java
View File

@@ -4,6 +4,7 @@
package frc.robot.commands.Elevateur; package frc.robot.commands.Elevateur;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Elevateur; import frc.robot.subsystems.Elevateur;
import frc.robot.subsystems.Pince; import frc.robot.subsystems.Pince;
@@ -12,6 +13,10 @@ import frc.robot.subsystems.Pince;
public class L4 extends Command { public class L4 extends Command {
private Elevateur elevateur; private Elevateur elevateur;
private Pince pince; private Pince pince;
private PIDController pidController;
private PIDController pidController1;
private double output;
private double output1;
/** Creates a new L2. */ /** Creates a new L2. */
public L4(Elevateur elevateur,Pince pince) { public L4(Elevateur elevateur,Pince pince) {
this.elevateur = elevateur; this.elevateur = elevateur;
@@ -22,30 +27,39 @@ public class L4 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.3, 0.04,0);
pidController1 = new PIDController(0.025, 0.00125,0.001, 0.01);
pidController.reset();
pidController1.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.encodeurelevateurL4bas() && elevateur.position()>=elevateur.encodeurelevateurL4haut()){ output = pidController.calculate(elevateur.position(),-6.3);
elevateur.vitesse(0); elevateur.vitesse(output);
pince.pivote(-0.15); output1 = pidController1.calculate(pince.encodeurpivot(),20);
if(pince.encodeurpivot()>=20 && pince.encodeurpivot()<=21){ pince.pivote(output1);
pince.pivote(0); // if(elevateur.position()<=elevateur.encodeurelevateurL4bas() && elevateur.position()>=elevateur.encodeurelevateurL4haut()){
} // elevateur.vitesse(0);
else if(pince.encodeurpivot()>=20){ // pince.pivote(-0.15);
pince.pivote(-0.15); // if(pince.encodeurpivot()>=20 && pince.encodeurpivot()<=21){
} // pince.pivote(0);
else{ // }
pince.pivote(0.15); // else if(pince.encodeurpivot()>=20){
} // pince.pivote(-0.15);
} // }
else if(elevateur.position()>=elevateur.encodeurelevateurL4bas()){ // else{
elevateur.vitesse(-0.7); // pince.pivote(0.15);
} // }
else{ // }
elevateur.vitesse(.25); // else if(elevateur.position()>=elevateur.encodeurelevateurL4bas()){
} // elevateur.vitesse(-0.7);
// }
// else{
// elevateur.vitesse(.25);
// }
} }
// Called once the command ends or is interrupted. // Called once the command ends or is interrupted.

10
src/main/java/frc/robot/commands/Limelight/AprilTag3.java
View File

@@ -47,14 +47,14 @@ public class AprilTag3 extends Command {
// 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() {
double a = limelight3.getX(); double a = limelight3.getTx();
double b = limelight3.getRx();
if(limelight3.getV() == true){ if(limelight3.getV() == true){
drivetrain.setControl(drive. drivetrain.setControl(drive.
withRotationalRate(a/10). withRotationalRate(a/10).
withVelocityX(x.getAsDouble()). withVelocityX(0).
withVelocityY(y.getAsDouble())); withVelocityY(b/10));
System.out.println(a/10); System.out.println(b/10);
} }
else{ else{
drivetrain.setControl(drive. drivetrain.setControl(drive.

2
src/main/java/frc/robot/commands/Limelight/Forme3.java
View File

@@ -46,7 +46,7 @@ public class Forme3 extends Command {
// 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() {
double a = limelight3.getX(); double a = limelight3.getTx();
if(limelight3.getV() == true){ if(limelight3.getV() == true){
drivetrain.setControl(drive. drivetrain.setControl(drive.
withRotationalRate(a/10). withRotationalRate(a/10).

6
src/main/java/frc/robot/subsystems/Elevateur.java
View File

@@ -31,10 +31,10 @@ public class Elevateur extends SubsystemBase {
teb.add("encodeurelevateursationhaut", -0.4).getEntry(); teb.add("encodeurelevateursationhaut", -0.4).getEntry();
private GenericEntry distanceDeploiePince = private GenericEntry distanceDeploiePince =
teb.add("encodeurDeploiePince", 0.2).getEntry(); teb.add("encodeurDeploiePince", 0.2).getEntry();
public Elevateur() { public Elevateur() {
teb.addDouble("encodeur elevateur",this::position); teb.addDouble("encodeur elevateur",this::position);
teb.addBoolean("limit elevateur", this::limit2); teb.addBoolean("limit elevateur", this::limit2);
teb.addDouble("amperage elevateur", this::amp);
} }
final SparkMax monte = new SparkMax(22, MotorType.kBrushless); final SparkMax monte = new SparkMax(22, MotorType.kBrushless);
final DigitalInput limit2 = new DigitalInput(0); final DigitalInput limit2 = new DigitalInput(0);
@@ -89,7 +89,9 @@ public class Elevateur extends SubsystemBase {
public double distanceDeploiePince(){ public double distanceDeploiePince(){
return distanceDeploiePince.getDouble(0.2); return distanceDeploiePince.getDouble(0.2);
} }
public double amp(){
return monte.getOutputCurrent();
}
@Override @Override
public void periodic() { public void periodic() {

11
src/main/java/frc/robot/subsystems/Limelight3.java
View File

@@ -20,8 +20,15 @@ public class Limelight3 extends SubsystemBase {
PortForwarder.add(port, "limelight.local", port); PortForwarder.add(port, "limelight.local", port);
} }
} }
public double getX(){ public double getTx(){
return LimelightHelpers.getTX("limelight-balon"); NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTableEntry tx = table.getEntry("tx");
return tx.getDouble(0.0);
}
public double getRx(){
NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon");
NetworkTableEntry rx = table.getEntry("rx");
return rx.getDouble(0.0);
} }
public boolean getV(){ public boolean getV(){
return LimelightHelpers.getTV("limelight-balon"); return LimelightHelpers.getTV("limelight-balon");

5
src/main/java/frc/robot/subsystems/Pince.java
View File

@@ -19,6 +19,7 @@ public class Pince extends SubsystemBase {
teb.addDouble("encodeur pince", this::encodeurpivot); teb.addDouble("encodeur pince", this::encodeurpivot);
teb.addDouble("amperage corail", this::emperagecoral); teb.addDouble("amperage corail", this::emperagecoral);
teb.addDouble("amperage algue", this::emperagealgue); teb.addDouble("amperage algue", this::emperagealgue);
teb.addDouble("amperage pivotis", this::ampPivote);
} }
final SparkMax coral = new SparkMax(20, MotorType.kBrushless); final SparkMax coral = new SparkMax(20, MotorType.kBrushless);
final SparkMax pivoti = new SparkMax(14, MotorType.kBrushless); final SparkMax pivoti = new SparkMax(14, MotorType.kBrushless);
@@ -63,7 +64,9 @@ public double emperagealgue(){
return algue1.getOutputCurrent(); return algue1.getOutputCurrent();
} }
public boolean x = false; public boolean x = false;
public double ampPivote(){
return pivoti.getOutputCurrent();
}
@Override @Override
public void periodic() { public void periodic() {
// This method will be called once per scheduler run // This method will be called once per scheduler run