diff --git a/build.gradle b/build.gradle index c6fea4f..ff9e9af 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.2.1" + id "edu.wpi.first.GradleRIO" version "2025.3.2" } java { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b49a82e..e0bf37a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -171,11 +171,11 @@ public class RobotContainer { .withVelocityY(0) .withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(2.35), drivetrain.applyRequest(()-> - drive.withVelocityX(-0.1*MaxSpeed) + drive.withVelocityX(0.1*MaxSpeed) .withVelocityY(0) .withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerR.getBoolean(true)).withTimeout(3), drivetrain.applyRequest(()-> - drive.withVelocityX(0.1*MaxSpeed) + drive.withVelocityX(-0.1*MaxSpeed) .withVelocityY(0) .withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)|| !ReculerB.getBoolean(true)).withTimeout(3), drivetrain.applyRequest(()-> 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 cb940c0..b0b0ae7 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L3.java +++ b/src/main/java/frc/robot/commands/Elevateur/L3.java @@ -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,10 @@ import frc.robot.subsystems.Pince; public class L3 extends Command { private Elevateur elevateur; private Pince pince; + 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; @@ -23,30 +27,39 @@ public class L3 extends Command { // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + 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() { - 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 = 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); + // 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. 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..e0de605 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.getTA()*100) / (Math.cos(90-limelight3.getTx())); if(limelight3.getV() == true){ drivetrain.setControl(drive. - withRotationalRate(a/10). - withVelocityX(x.getAsDouble()). - withVelocityY(y.getAsDouble())); - System.out.println(a/10); + withRotationalRate(a/20). + withVelocityX(0). + withVelocityY( b/50)); + System.out.println(b/50); } 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/Bougie.java b/src/main/java/frc/robot/subsystems/Bougie.java index d5df941..bae7785 100644 --- a/src/main/java/frc/robot/subsystems/Bougie.java +++ b/src/main/java/frc/robot/subsystems/Bougie.java @@ -6,14 +6,22 @@ package frc.robot.subsystems; import com.ctre.phoenix.led.CANdle; import com.ctre.phoenix.led.CANdleConfiguration; +import com.ctre.phoenix.led.FireAnimation; +import com.ctre.phoenix.led.LarsonAnimation; import com.ctre.phoenix.led.RainbowAnimation; +import com.ctre.phoenix.led.TwinkleAnimation; +import com.ctre.phoenix.led.TwinkleOffAnimation; +import com.ctre.phoenix.led.TwinkleOffAnimation.TwinkleOffPercent; +import com.ctre.phoenix.led.LarsonAnimation.BounceMode; +import com.ctre.phoenix.led.TwinkleAnimation.TwinklePercent; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Bougie extends SubsystemBase { CANdle candle = new CANdle(23); CANdleConfiguration config = new CANdleConfiguration(); - RainbowAnimation rainbowAnim = new RainbowAnimation(1, 0.5, 68); + LarsonAnimation rainbowAnim = new LarsonAnimation(256,0,0,0,0.1,68,BounceMode.Front,10,8); + //TwinkleOffAnimation rainbowAnim = new TwinkleOffAnimation(256, 0, 0,0,0.5,68,TwinkleOffPercent.Percent88,8); int led = 16; int led2 = 8; boolean x =true; diff --git a/src/main/java/frc/robot/subsystems/Elevateur.java b/src/main/java/frc/robot/subsystems/Elevateur.java index 1ef7681..314d3f7 100644 --- a/src/main/java/frc/robot/subsystems/Elevateur.java +++ b/src/main/java/frc/robot/subsystems/Elevateur.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.SubsystemBase; + import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType; public class Elevateur extends SubsystemBase { @@ -31,14 +32,13 @@ 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); - public double position(){ return monte.getEncoder().getPosition(); } @@ -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..75dd7ce 100644 --- a/src/main/java/frc/robot/subsystems/Limelight3.java +++ b/src/main/java/frc/robot/subsystems/Limelight3.java @@ -20,8 +20,20 @@ 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 getTA(){ + NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight-balon"); + NetworkTableEntry ta = table.getEntry("ta"); + return ta.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