diff --git a/src/main/java/frc/robot/commands/Elevateur/L2.java b/src/main/java/frc/robot/commands/Elevateur/L2.java index 9b60cea..d550b5b 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L2.java +++ b/src/main/java/frc/robot/commands/Elevateur/L2.java @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.commands.Elevateur; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Elevateur; import frc.robot.subsystems.Pince; @@ -11,6 +12,8 @@ import frc.robot.subsystems.Pince; public class L2 extends Command { private Elevateur elevateur; private Pince pince; + private PIDController pidController; + private double output; /** Creates a new L2. */ public L2(Elevateur elevateur,Pince pince) { this.elevateur = elevateur; @@ -21,20 +24,24 @@ public class L2 extends Command { // Called when the command is initially scheduled. @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. @Override public void execute() { - if(pince.encodeurpivot()>=15 && pince.encodeurpivot()<=16.5){ - pince.pivote(0); - } - else if(pince.encodeurpivot()>=15){ - pince.pivote(-0.2); - } - else{ - pince.pivote(0.1); - } + output = pidController.calculate(pince.encodeurpivot(),15); + pince.pivote(output); + // if(pince.encodeurpivot()>=15 && pince.encodeurpivot()<=16.5){ + // pince.pivote(0); + // } + // else if(pince.encodeurpivot()>=15){ + // pince.pivote(-0.2); + // } + // else{ + // pince.pivote(0.1); + // } } diff --git a/src/main/java/frc/robot/commands/Elevateur/L3.java b/src/main/java/frc/robot/commands/Elevateur/L3.java index 8011b68..b0b0ae7 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L3.java +++ b/src/main/java/frc/robot/commands/Elevateur/L3.java @@ -13,8 +13,10 @@ import frc.robot.subsystems.Pince; public class L3 extends Command { private Elevateur elevateur; private Pince pince; - private PIDController pidController; + private PIDController pidController1; + private PIDController pidController2; private double output; + private double output2; /** Creates a new L2. */ public L3(Elevateur elevateur,Pince pince) { this.elevateur = elevateur; @@ -26,15 +28,19 @@ public class L3 extends Command { // Called when the command is initially scheduled. @Override public void initialize() { - pidController = new PIDController(0.03, 0.001, 0.001); - pidController.reset(); + pidController1 = new PIDController(0.3, 0.04,0); + 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. @Override public void execute() { - output = pidController.calculate(elevateur.position(),20); + output = pidController1.calculate(elevateur.position(),-3); + output2 = pidController2.calculate(pince.encodeurpivot(),20); elevateur.vitesse(output); + pince.pivote(output2); // if(elevateur.position()<=elevateur.encodeurelevateurL3bas() && elevateur.position()>=elevateur.encodeurelevateurL3haut()){ // elevateur.vitesse(0); // pince.pivote(-0.15); diff --git a/src/main/java/frc/robot/commands/Elevateur/L4.java b/src/main/java/frc/robot/commands/Elevateur/L4.java index 4b8dcbc..0ae6fe1 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L4.java +++ b/src/main/java/frc/robot/commands/Elevateur/L4.java @@ -4,6 +4,7 @@ package frc.robot.commands.Elevateur; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Elevateur; import frc.robot.subsystems.Pince; @@ -12,6 +13,10 @@ import frc.robot.subsystems.Pince; public class L4 extends Command { private Elevateur elevateur; private Pince pince; + private PIDController pidController; + private PIDController pidController1; + private double output; + private double output1; /** Creates a new L2. */ public L4(Elevateur elevateur,Pince pince) { this.elevateur = elevateur; @@ -22,30 +27,39 @@ public class L4 extends Command { // Called when the command is initially scheduled. @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. @Override public void execute() { - if(elevateur.position()<=elevateur.encodeurelevateurL4bas() && elevateur.position()>=elevateur.encodeurelevateurL4haut()){ - 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.encodeurelevateurL4bas()){ - elevateur.vitesse(-0.7); - } - else{ - elevateur.vitesse(.25); - } + output = pidController.calculate(elevateur.position(),-6.3); + elevateur.vitesse(output); + output1 = pidController1.calculate(pince.encodeurpivot(),20); + pince.pivote(output1); + // if(elevateur.position()<=elevateur.encodeurelevateurL4bas() && elevateur.position()>=elevateur.encodeurelevateurL4haut()){ + // 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.encodeurelevateurL4bas()){ + // elevateur.vitesse(-0.7); + // } + // else{ + // elevateur.vitesse(.25); + // } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java index 4253b96..7dd2172 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java @@ -47,14 +47,14 @@ public class AprilTag3 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double a = limelight3.getX(); - + double a = limelight3.getTx(); + double b = limelight3.getRx(); if(limelight3.getV() == true){ drivetrain.setControl(drive. withRotationalRate(a/10). - withVelocityX(x.getAsDouble()). - withVelocityY(y.getAsDouble())); - System.out.println(a/10); + withVelocityX(0). + withVelocityY(b/10)); + System.out.println(b/10); } else{ drivetrain.setControl(drive. diff --git a/src/main/java/frc/robot/commands/Limelight/Forme3.java b/src/main/java/frc/robot/commands/Limelight/Forme3.java index 2504d38..8afaec4 100644 --- a/src/main/java/frc/robot/commands/Limelight/Forme3.java +++ b/src/main/java/frc/robot/commands/Limelight/Forme3.java @@ -46,7 +46,7 @@ public class Forme3 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double a = limelight3.getX(); + double a = limelight3.getTx(); if(limelight3.getV() == true){ drivetrain.setControl(drive. withRotationalRate(a/10). diff --git a/src/main/java/frc/robot/subsystems/Elevateur.java b/src/main/java/frc/robot/subsystems/Elevateur.java index 1ef7681..06c9250 100644 --- a/src/main/java/frc/robot/subsystems/Elevateur.java +++ b/src/main/java/frc/robot/subsystems/Elevateur.java @@ -31,10 +31,10 @@ public class Elevateur extends SubsystemBase { teb.add("encodeurelevateursationhaut", -0.4).getEntry(); private GenericEntry distanceDeploiePince = teb.add("encodeurDeploiePince", 0.2).getEntry(); - public Elevateur() { teb.addDouble("encodeur elevateur",this::position); teb.addBoolean("limit elevateur", this::limit2); + teb.addDouble("amperage elevateur", this::amp); } final SparkMax monte = new SparkMax(22, MotorType.kBrushless); final DigitalInput limit2 = new DigitalInput(0); @@ -89,7 +89,9 @@ public class Elevateur extends SubsystemBase { public double distanceDeploiePince(){ return distanceDeploiePince.getDouble(0.2); } - + public double amp(){ + return monte.getOutputCurrent(); + } @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/Limelight3.java b/src/main/java/frc/robot/subsystems/Limelight3.java index fe039c4..787fc37 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3.java +++ b/src/main/java/frc/robot/subsystems/Limelight3.java @@ -20,8 +20,15 @@ public class Limelight3 extends SubsystemBase { PortForwarder.add(port, "limelight.local", port); } } - public double getX(){ - return LimelightHelpers.getTX("limelight-balon"); + public double getTx(){ + 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(){ return LimelightHelpers.getTV("limelight-balon"); diff --git a/src/main/java/frc/robot/subsystems/Pince.java b/src/main/java/frc/robot/subsystems/Pince.java index 4ddc0a7..a75cb5f 100644 --- a/src/main/java/frc/robot/subsystems/Pince.java +++ b/src/main/java/frc/robot/subsystems/Pince.java @@ -19,6 +19,7 @@ public class Pince extends SubsystemBase { teb.addDouble("encodeur pince", this::encodeurpivot); teb.addDouble("amperage corail", this::emperagecoral); teb.addDouble("amperage algue", this::emperagealgue); + teb.addDouble("amperage pivotis", this::ampPivote); } final SparkMax coral = new SparkMax(20, MotorType.kBrushless); final SparkMax pivoti = new SparkMax(14, MotorType.kBrushless); @@ -63,7 +64,9 @@ public double emperagealgue(){ return algue1.getOutputCurrent(); } public boolean x = false; - +public double ampPivote(){ + return pivoti.getOutputCurrent(); +} @Override public void periodic() { // This method will be called once per scheduler run