From c0e7985ab7ef30fb4cf683ce5960f6ed9310048e Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Tue, 4 Mar 2025 17:37:15 -0500 Subject: [PATCH] mode auto --- src/main/java/frc/robot/RobotContainer.java | 20 +++--- .../java/frc/robot/commands/Elevateur/L2.java | 4 +- .../robot/commands/requin/BalayeuseAlgue.java | 4 +- .../robot/commands/requin/ExpireCorail.java | 4 +- .../frc/robot/commands/requin/L1Requin.java | 4 +- .../subsystems/CommandSwerveDrivetrain.java | 72 +++++++++---------- 6 files changed, 54 insertions(+), 54 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cdb8827..24ba9d7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -78,7 +79,6 @@ public class RobotContainer { private final CommandXboxController manette2 = new CommandXboxController(1); private final Pigeon2 gyro = new Pigeon2(13); // ID du Pigeon 2 public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - //private final SendableChooser autoChooser; public double getAngle() { return gyro.getYaw().getValueAsDouble(); // Retourne l'angle actuel du robot } @@ -160,24 +160,24 @@ public class RobotContainer { drivetrain.applyRequest(()-> drive.withVelocityX(-0.1*MaxSpeed) .withVelocityY(0) - .withRotationalRate(0)).unless(()->!Reculer.getBoolean(true)).withTimeout(3), - drivetrain.applyRequest(()-> - drive.withVelocityX(0.5*MaxSpeed) - .withVelocityY(0) - .withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(3), + .withRotationalRate(0)).unless(()->!Reculer.getBoolean(true)).withTimeout(3.5), + drivetrain.applyRequest(()-> + drive.withVelocityX(0.1*MaxSpeed) + .withVelocityY(0) + .withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(3.5), drivetrain.applyRequest(()-> - drive.withVelocityX(-0.5*MaxSpeed) + drive.withVelocityX(-0.1*MaxSpeed) .withVelocityY(0) - .withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)).withTimeout(2), + .withRotationalRate(0)).unless(()->!sortirAngle.getBoolean(true)).withTimeout(3), drivetrain.applyRequest(()-> drive.withVelocityX(0) .withVelocityY(0) .withRotationalRate(0)).withTimeout(0.1), new L4(elevateur, pince).unless(()->!L4.getBoolean(true)).withTimeout(4), drivetrain.applyRequest(()-> - drive.withVelocityX(0.2*MaxSpeed) + drive.withVelocityX(0.1*MaxSpeed) .withVelocityY(0) - .withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(0.5), + .withRotationalRate(0)).unless(()->!L4.getBoolean(true)).withTimeout(0.25), drivetrain.applyRequest(()-> drive.withVelocityX(0*MaxSpeed) .withVelocityY(0) diff --git a/src/main/java/frc/robot/commands/Elevateur/L2.java b/src/main/java/frc/robot/commands/Elevateur/L2.java index 99b1a3a..9b60cea 100644 --- a/src/main/java/frc/robot/commands/Elevateur/L2.java +++ b/src/main/java/frc/robot/commands/Elevateur/L2.java @@ -26,10 +26,10 @@ public class L2 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(pince.encodeurpivot()>=14 && pince.encodeurpivot()<=15){ + if(pince.encodeurpivot()>=15 && pince.encodeurpivot()<=16.5){ pince.pivote(0); } - else if(pince.encodeurpivot()>=14){ + else if(pince.encodeurpivot()>=15){ pince.pivote(-0.2); } else{ diff --git a/src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java b/src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java index f6130df..1a2216d 100644 --- a/src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java +++ b/src/main/java/frc/robot/commands/requin/BalayeuseAlgue.java @@ -27,8 +27,8 @@ public class BalayeuseAlgue extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double cibleMin = 700; - double cibleMax = 900; + double cibleMin = 550; + double cibleMax = 650; if(requin.amp()>=78.2){ requin.xRequin = true; diff --git a/src/main/java/frc/robot/commands/requin/ExpireCorail.java b/src/main/java/frc/robot/commands/requin/ExpireCorail.java index 3dd81c1..ba8f98b 100644 --- a/src/main/java/frc/robot/commands/requin/ExpireCorail.java +++ b/src/main/java/frc/robot/commands/requin/ExpireCorail.java @@ -28,12 +28,12 @@ public class ExpireCorail extends Command { @Override public void execute() { if(requin.amp()> 60){ - requin.balaye(-0.4); + requin.balaye(-0.2); } else { bougie.Rouge(); - requin.balaye(-0.4); + requin.balaye(-0.2); } } diff --git a/src/main/java/frc/robot/commands/requin/L1Requin.java b/src/main/java/frc/robot/commands/requin/L1Requin.java index dc6b86f..bbae1d4 100644 --- a/src/main/java/frc/robot/commands/requin/L1Requin.java +++ b/src/main/java/frc/robot/commands/requin/L1Requin.java @@ -27,10 +27,10 @@ public class L1Requin extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if(requin.encodeur()<=530 && requin.encodeur()>=430){ + if(requin.encodeur()<=485 && requin.encodeur()>=385){ requin.rotationer(0); } - else if(requin.encodeur()>=530){ + else if(requin.encodeur()>=485){ requin.rotationer(-0.5); } else{ diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 4a103a9..9d3b2e0 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -45,42 +45,42 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - private void configureAutoBuilder() { - try { - var config = RobotConfig.fromGUISettings(); - AutoBuilder.configure( + // private void configureAutoBuilder() { + // try { + // var config = RobotConfig.fromGUISettings(); + // AutoBuilder.configure( - () -> getState().Pose, // Supplier of current robot pose - this::resetPose, // Consumer for seeding pose against auto - () -> getState().Speeds, // Supplier of current robot speeds + // () -> getState().Pose, // Supplier of current robot pose + // this::resetPose, // Consumer for seeding pose against auto + // () -> getState().Speeds, // Supplier of current robot speeds - // Consumer of ChassisSpeeds and feedforwards to drive the robot - (speeds, feedforwards) -> setControl( - m_pathApplyRobotSpeeds.withSpeeds(speeds) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesYNewtons()) - ), - new PPHolonomicDriveController( - // PID constants for translation - new PIDConstants(63.167, 0, 0.54521), - // // PID constants for rotation - // new PIDConstants(7.9735, 0, 0.038499) - // PID constants for rotation - new PIDConstants(43.502,0,2.7353) - ), - config, - // Assume the path needs to be flipped for Red vs Blue, this is normally the case - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this // Subsystem for requirements + // // Consumer of ChassisSpeeds and feedforwards to drive the robot + // (speeds, feedforwards) -> setControl( + // m_pathApplyRobotSpeeds.withSpeeds(speeds) + // .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesXNewtons()) + // .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesYNewtons()) + // ), + // new PPHolonomicDriveController( + // // PID constants for translation + // new PIDConstants(63.167, 0, 0.54521), + // // // PID constants for rotation + // // new PIDConstants(7.9735, 0, 0.038499) + // // PID constants for rotation + // new PIDConstants(43.502,0,2.7353) + // ), + // config, + // // Assume the path needs to be flipped for Red vs Blue, this is normally the case + // () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + // this // Subsystem for requirements - ); - } catch (Exception ex) { - DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); - } - PPHolonomicDriveController.overrideRotationFeedback(()->{ - return 0; - }); - } + // ); + // } catch (Exception ex) { + // DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); + // } + // PPHolonomicDriveController.overrideRotationFeedback(()->{ + // return 0; + // }); + // } /** * Constructs a CTRE SwerveDrivetrain using the specified constants. @@ -100,7 +100,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su if (Utils.isSimulation()) { startSimThread(); } - configureAutoBuilder(); + //configureAutoBuilder(); } /** @@ -125,7 +125,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su if (Utils.isSimulation()) { startSimThread(); } - configureAutoBuilder(); + // configureAutoBuilder(); } /** @@ -158,7 +158,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su if (Utils.isSimulation()) { startSimThread(); } - configureAutoBuilder(); + //configureAutoBuilder(); }