From 92c1c59599dd97ecf5e2cd1f87c5205a5df8154b Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Wed, 3 Dec 2025 17:57:22 -0500 Subject: [PATCH 1/5] --- .../deploy/pathplanner/paths/New Path.path | 10 +++++----- .../subsystems/CommandSwerveDrivetrain.java | 19 +++++++++++-------- 2 files changed, 16 insertions(+), 13 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 601f47e..d23e318 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 16.243499999999997, + "x": 16.243, "y": 6.55025 }, "prevControl": null, "nextControl": { - "x": 15.18625440479427, - "y": 6.55025 + "x": 15.354412756981612, + "y": 6.548572497113415 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 6.55025 }, "prevControl": { - "x": 2.34525, - "y": 6.55025 + "x": 2.306674365666888, + "y": 6.526815183428469 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 373a09e..0e8cf01 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -10,6 +10,7 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -44,12 +45,13 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - + public void resetPose(Pose2d newPose) { + getState().Pose = newPose; // ou appelle setPose(newPose) selon ton code + }//ca marche pas private void configureAutoBuilder() { try { var config = RobotConfig.fromGUISettings(); - AutoBuilder.configure( - + AutoBuilder.configure( () -> getState().Pose, // Supplier of current robot pose this::resetPose, // Consumer for seeding pose against auto () -> getState().Speeds, // Supplier of current robot speeds @@ -77,10 +79,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su } catch (Exception ex) { DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); } - PPHolonomicDriveController.overrideRotationFeedback(()->{ - return 0; - }); + PPHolonomicDriveController.overrideRotationFeedback(()->{ + return 0; + }); } + /** * Constructs a CTRE SwerveDrivetrain using the specified constants. @@ -125,7 +128,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su if (Utils.isSimulation()) { startSimThread(); } - // configureAutoBuilder(); + configureAutoBuilder(); } /** @@ -158,7 +161,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su if (Utils.isSimulation()) { startSimThread(); } - //configureAutoBuilder(); + configureAutoBuilder(); } From 0f9a71c3759c2a3aa13f8147cd3eff45d83778f6 Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Mon, 8 Dec 2025 18:29:44 -0500 Subject: [PATCH 2/5] mode auto dans subsystem fini --- simgui-ds.json | 4 +- .../robot/commands/Limelight/AprilTag3.java | 1 + .../subsystems/CommandSwerveDrivetrain.java | 164 +++++++++++------- ....2.3.json => PathplannerLib-2025.2.7.json} | 8 +- 4 files changed, 111 insertions(+), 66 deletions(-) rename vendordeps/{PathplannerLib-2025.2.3.json => PathplannerLib-2025.2.7.json} (87%) diff --git a/simgui-ds.json b/simgui-ds.json index 4a63cc1..4919a24 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,7 +1,7 @@ { - "System Joysticks": { + "Joysticks": { "window": { - "enabled": false + "visible": false } }, "keyboardJoysticks": [ diff --git a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java index 81d297c..22d141e 100644 --- a/src/main/java/frc/robot/commands/Limelight/AprilTag3.java +++ b/src/main/java/frc/robot/commands/Limelight/AprilTag3.java @@ -76,6 +76,7 @@ public class AprilTag3 extends Command { withRotationalRate(tx/20). withVelocityX(2-botx). withVelocityY(2-boty)); + } } else{ diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 47d5a05..fc3a8a5 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -1,9 +1,12 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.MetersPerSecond; + import java.util.function.Supplier; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; @@ -14,7 +17,11 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.DriverStation; @@ -23,6 +30,7 @@ import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; +import frc.robot.TunerConstants.TunerConstants; import frc.robot.TunerConstants.TunerConstants.TunerSwerveDrivetrain; /** @@ -44,51 +52,71 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private SwerveDriveOdometry odometry; private SwerveDrivePoseEstimator poseEstimator; private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); - +// Position des modules par rapport au centre du robot +Translation2d frontLeftLocation = new Translation2d(+0.3, +0.3); +Translation2d frontRightLocation = new Translation2d(+0.3, -0.3); +Translation2d backLeftLocation = new Translation2d(-0.3, +0.3); +Translation2d backRightLocation = new Translation2d(-0.3, -0.3); +private final SwerveDriveKinematics kinematics = + new SwerveDriveKinematics( + frontLeftLocation, + frontRightLocation, + backLeftLocation, + backRightLocation + ); /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - public void resetPose(Pose2d newPose) { - getState().Pose = newPose; // ou appelle setPose(newPose) selon ton code - }//ca marche pas - private void configureAutoBuilder() { - try { - var config = RobotConfig.fromGUISettings(); - AutoBuilder.configure( - () -> getState().Pose, // Supplier of current robot pose - this::resetOdometry, // Consumer for seeding pose against auto - () -> getState().Speeds, // Supplier of current robot speeds + // 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 - // 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(5, 0, 0), - // // PID constants for rotation - // new PIDConstants(7.9735, 0, 0.038499) - // PID constants for rotation - new PIDConstants(5,0,0) - ), - 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(5, 0, 0), + // // // PID constants for rotation + // // new PIDConstants(7.9735, 0, 0.038499) + // // PID constants for rotation + // new PIDConstants(5,0,0) + // ), + // 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; + // }); + // } +// public void driveRobotRelative(ChassisSpeeds speeds) { +// // Convertir robot-relative ChassisSpeeds → SwerveModuleStates +// SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds); + +// // Limitations des roues +// SwerveDriveKinematics.desaturateWheelSpeeds(states, TunerConstants.kSpeedAt12Volts); + +// // Envoi aux modules +// frontLeft.setDesiredState(states[0]); +// frontRight.setDesiredState(states[1]); +// backLeft.setDesiredState(states[2]); +// backRight.setDesiredState(states[3]); +// } /** * Constructs a CTRE SwerveDrivetrain using the specified constants. *

@@ -107,27 +135,43 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su if (Utils.isSimulation()) { startSimThread(); } - configureAutoBuilder(); - } - // public void resetPose(Pose2d pose) { - // // Reset gyro to match starting rotation - // pigeon2.setYaw(pose.getRotation().getDegrees()); - - // // Reset odometry - // m_odometry.resetPosition( - // pose.getRotation(), - // getState().ModulePositions, - // pose - // ); - //} - public void resetOdometry(Pose2d pose) { - odometry = new SwerveDriveOdometry(getKinematics(), kBlueAlliancePerspectiveRotation, getState().ModulePositions); - odometry.resetPosition( - getPigeon2().getRotation2d(), - getState().ModulePositions, - pose + RobotConfig config; + System.out.println("avant le mode auto est genere"); + try{ + config = RobotConfig.fromGUISettings(); + SwerveRequest.ApplyRobotSpeeds driveAuto = new SwerveRequest.ApplyRobotSpeeds().withDriveRequestType(DriveRequestType.OpenLoopVoltage); + // Configure AutoBuilder last + AutoBuilder.configure( + () -> getState().Pose, // Robot pose supplier + this::resetPose, // Method to reset odometry (will be called if your auto has a starting pose) + () -> getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds, feedforwards) -> this.applyRequest(()-> driveAuto.withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards + new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants + ), + config, // The robot configuration + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements ); - this.getPigeon2().setYaw(pose.getRotation().getDegrees()); + System.out.println("le mode auto est genere"); + } + catch (Exception e) { + // Handle exception as needed + e.printStackTrace(); + System.out.println("le mode auto ne marche pas"); + } + // configureAutoBuilder(); } /** * Constructs a CTRE SwerveDrivetrain using the specified constants. @@ -151,7 +195,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su if (Utils.isSimulation()) { startSimThread(); } - // configureAutoBuilder(); + // configureAutoBuilder(); } /** @@ -184,7 +228,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su if (Utils.isSimulation()) { startSimThread(); } - configureAutoBuilder(); + // configureAutoBuilder(); } diff --git a/vendordeps/PathplannerLib-2025.2.3.json b/vendordeps/PathplannerLib-2025.2.7.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.3.json rename to vendordeps/PathplannerLib-2025.2.7.json index 9151ce4..d3f84e5 100644 --- a/vendordeps/PathplannerLib-2025.2.3.json +++ b/vendordeps/PathplannerLib-2025.2.7.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.3.json", + "fileName": "PathplannerLib-2025.2.7.json", "name": "PathplannerLib", - "version": "2025.2.3", + "version": "2025.2.7", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.3" + "version": "2025.2.7" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.3", + "version": "2025.2.7", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From 5e9f2f5244dbc5edad720054bdd427a93ce9eee6 Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Mon, 8 Dec 2025 19:23:02 -0500 Subject: [PATCH 3/5] meillieur subsytem swerve (pas tent) --- simgui-ds.json | 5 + .../pathplanner/paths/BlueBasChercher.path | 6 +- src/main/java/frc/robot/RobotContainer.java | 5 - .../subsystems/CommandSwerveDrivetrain.java | 112 ++++++++--------- ....2.2.json => Phoenix6-frc2025-latest.json} | 118 +++++++++++++----- 5 files changed, 146 insertions(+), 100 deletions(-) rename vendordeps/{Phoenix6-25.2.2.json => Phoenix6-frc2025-latest.json} (80%) diff --git a/simgui-ds.json b/simgui-ds.json index 4919a24..1ba71ce 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -4,6 +4,11 @@ "visible": false } }, + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/deploy/pathplanner/paths/BlueBasChercher.path b/src/main/deploy/pathplanner/paths/BlueBasChercher.path index 7a2e9c8..be8a2f6 100644 --- a/src/main/deploy/pathplanner/paths/BlueBasChercher.path +++ b/src/main/deploy/pathplanner/paths/BlueBasChercher.path @@ -11,7 +11,7 @@ "x": 4.699180327868851, "y": 2.1728995901639334 }, - "isLocked": false, + "isLocked": true, "linkedName": null }, { @@ -27,7 +27,7 @@ "x": 2.853313965378558, "y": 1.5363479888253109 }, - "isLocked": false, + "isLocked": true, "linkedName": null }, { @@ -40,7 +40,7 @@ "y": 1.3902362024139756 }, "nextControl": null, - "isLocked": false, + "isLocked": true, "linkedName": null } ], diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a5477ff..6b39345 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,7 +14,6 @@ import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; @@ -23,12 +22,9 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 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; import frc.robot.TunerConstants.TunerConstants; -import frc.robot.commands.RainBow; import frc.robot.commands.reset; import frc.robot.commands.Elevateur.Depart; import frc.robot.commands.Elevateur.ElevateurManuel; @@ -96,7 +92,6 @@ public class RobotContainer { Bougie bougie = new Bougie(); Limelight3G limelight3g = new Limelight3G(); Limelight3 limelight3 = new Limelight3(); - Pose2d pose = new Pose2d(); Requin requin = new Requin(); CorailAspir corailAspir = new CorailAspir(pince, bougie); public RobotContainer() { diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index fc3a8a5..886e746 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -1,12 +1,9 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.MetersPerSecond; import java.util.function.Supplier; import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; @@ -14,14 +11,9 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.kinematics.SwerveDriveKinematics; -import edu.wpi.first.math.kinematics.SwerveDriveOdometry; -import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.DriverStation; @@ -30,7 +22,6 @@ import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.robot.TunerConstants.TunerConstants; import frc.robot.TunerConstants.TunerConstants.TunerSwerveDrivetrain; /** @@ -42,33 +33,64 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private static final double kSimLoopPeriod = 0.005; // 5 ms private Notifier m_simNotifier = null; private double m_lastSimTime; - private Pigeon2 pigeon2 = new Pigeon2(13); /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; /* Keep track if we've ever applied the operator perspective before or not */ private boolean m_hasAppliedOperatorPerspective = false; - private SwerveDriveOdometry odometry; - private SwerveDrivePoseEstimator poseEstimator; private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); // Position des modules par rapport au centre du robot Translation2d frontLeftLocation = new Translation2d(+0.3, +0.3); Translation2d frontRightLocation = new Translation2d(+0.3, -0.3); Translation2d backLeftLocation = new Translation2d(-0.3, +0.3); Translation2d backRightLocation = new Translation2d(-0.3, -0.3); -private final SwerveDriveKinematics kinematics = - new SwerveDriveKinematics( - frontLeftLocation, - frontRightLocation, - backLeftLocation, - backRightLocation - ); /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - // private void configureAutoBuilder() { + private void configureAutoBuilder() { + RobotConfig config; + System.out.println("avant le mode auto est genere"); + try{ + config = RobotConfig.fromGUISettings(); + // Configure AutoBuilder last + AutoBuilder.configure( + () -> getState().Pose, // Robot pose supplier + (pose) -> { + System.out.println("reset Pose " + pose.getX() + " " + pose.getY()); + this.resetPose(pose); + this.resetPose(pose); + }, // Method to reset odometry (will be called if your auto has a starting pose) + () -> getState().Speeds, + (speeds, feedforwards) -> setControl( + m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020)) + .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) + ), // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + //(speeds, feedforwards) -> this.applyRequest(()-> driveAuto.withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards + new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants + ), + config, // The robot configuration + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + System.out.println("le mode auto est genere"); + } + catch (Exception e) { + // Handle exception as needed + e.printStackTrace(); + System.out.println("le mode auto ne marche pas"); + } // try { // var config = RobotConfig.fromGUISettings(); // AutoBuilder.configure( @@ -102,7 +124,7 @@ private final SwerveDriveKinematics kinematics = // PPHolonomicDriveController.overrideRotationFeedback(()->{ // return 0; // }); - // } + } // public void driveRobotRelative(ChassisSpeeds speeds) { // // Convertir robot-relative ChassisSpeeds → SwerveModuleStates @@ -135,43 +157,7 @@ private final SwerveDriveKinematics kinematics = if (Utils.isSimulation()) { startSimThread(); } - RobotConfig config; - System.out.println("avant le mode auto est genere"); - try{ - config = RobotConfig.fromGUISettings(); - SwerveRequest.ApplyRobotSpeeds driveAuto = new SwerveRequest.ApplyRobotSpeeds().withDriveRequestType(DriveRequestType.OpenLoopVoltage); - // Configure AutoBuilder last - AutoBuilder.configure( - () -> getState().Pose, // Robot pose supplier - this::resetPose, // Method to reset odometry (will be called if your auto has a starting pose) - () -> getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds, feedforwards) -> this.applyRequest(()-> driveAuto.withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards - new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains - new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants - ), - config, // The robot configuration - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this // Reference to this subsystem to set requirements - ); - System.out.println("le mode auto est genere"); - } - catch (Exception e) { - // Handle exception as needed - e.printStackTrace(); - System.out.println("le mode auto ne marche pas"); - } - // configureAutoBuilder(); + configureAutoBuilder(); } /** * Constructs a CTRE SwerveDrivetrain using the specified constants. @@ -195,7 +181,7 @@ private final SwerveDriveKinematics kinematics = if (Utils.isSimulation()) { startSimThread(); } - // configureAutoBuilder(); + configureAutoBuilder(); } /** @@ -228,7 +214,7 @@ private final SwerveDriveKinematics kinematics = if (Utils.isSimulation()) { startSimThread(); } - // configureAutoBuilder(); + configureAutoBuilder(); } diff --git a/vendordeps/Phoenix6-25.2.2.json b/vendordeps/Phoenix6-frc2025-latest.json similarity index 80% rename from vendordeps/Phoenix6-25.2.2.json rename to vendordeps/Phoenix6-frc2025-latest.json index d617643..6f40c84 100644 --- a/vendordeps/Phoenix6-25.2.2.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.2.2.json", + "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.2", + "version": "25.4.0", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.2" + "version": "25.4.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,35 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.2", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +238,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +254,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +270,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +286,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +302,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +318,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +334,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +350,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +366,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +382,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +398,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +414,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +430,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -414,6 +442,38 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } \ No newline at end of file From fa307579901b9e6f903c2746fa92fe2fb497d31d Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Wed, 10 Dec 2025 18:31:12 -0500 Subject: [PATCH 4/5] auto I MARCHE(pas) --- .../subsystems/CommandSwerveDrivetrain.java | 60 +++++++------------ ....2.7.json => PathplannerLib-2025.2.6.json} | 8 +-- 2 files changed, 25 insertions(+), 43 deletions(-) rename vendordeps/{PathplannerLib-2025.2.7.json => PathplannerLib-2025.2.6.json} (87%) diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 886e746..274f3f5 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -47,50 +47,32 @@ Translation2d backLeftLocation = new Translation2d(-0.3, +0.3); Translation2d backRightLocation = new Translation2d(-0.3, -0.3); /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ private void configureAutoBuilder() { - RobotConfig config; - System.out.println("avant le mode auto est genere"); - try{ - config = RobotConfig.fromGUISettings(); - // Configure AutoBuilder last - AutoBuilder.configure( - () -> getState().Pose, // Robot pose supplier - (pose) -> { - System.out.println("reset Pose " + pose.getX() + " " + pose.getY()); - this.resetPose(pose); - this.resetPose(pose); - }, // Method to reset odometry (will be called if your auto has a starting pose) - () -> getState().Speeds, + 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 + // Consumer of ChassisSpeeds and feedforwards to drive the robot (speeds, feedforwards) -> setControl( m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020)) .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) - ), // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - //(speeds, feedforwards) -> this.applyRequest(()-> driveAuto.withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards - new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains - new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants ), - config, // The robot configuration - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this // Reference to this subsystem to set requirements - ); - System.out.println("le mode auto est genere"); - } - catch (Exception e) { - // Handle exception as needed - e.printStackTrace(); - System.out.println("le mode auto ne marche pas"); - } + new PPHolonomicDriveController( + // PID constants for translation + new PIDConstants(10, 0, 0), + // PID constants for rotation + new PIDConstants(7, 0, 0) + ), + 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()); + } // try { // var config = RobotConfig.fromGUISettings(); // AutoBuilder.configure( diff --git a/vendordeps/PathplannerLib-2025.2.7.json b/vendordeps/PathplannerLib-2025.2.6.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.7.json rename to vendordeps/PathplannerLib-2025.2.6.json index d3f84e5..95ba203 100644 --- a/vendordeps/PathplannerLib-2025.2.7.json +++ b/vendordeps/PathplannerLib-2025.2.6.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.7.json", + "fileName": "PathplannerLib-2025.2.6.json", "name": "PathplannerLib", - "version": "2025.2.7", + "version": "2025.2.6", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.7" + "version": "2025.2.6" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.7", + "version": "2025.2.6", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From 7032b95c6071170a326f4b94222511ab6bf357f8 Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Wed, 10 Dec 2025 19:01:01 -0500 Subject: [PATCH 5/5] LE MODE AUTO MARCHE --- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6b39345..d9a83c0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -102,7 +102,6 @@ public class RobotContainer { } private void configureBindings() { - drivetrain.registerTelemetry(logger::telemeterize); drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically drivetrain.applyRequest(() -> @@ -117,7 +116,7 @@ public class RobotContainer { manette1.rightTrigger().whileTrue(new CoralAlgueInspire(pince, bougie)); manette1.rightBumper().toggleOnTrue(new StationPince(pince, elevateur,bougie)); manette1.povLeft().whileTrue(new AlgueExpire(pince, bougie)); - manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain,manette1::getLeftX,manette1::getLeftY)); + // manette1.leftBumper().whileTrue(new AprilTag3G(limelight3g,drivetrain1,manette1::getLeftX,manette1::getLeftY)); manette1.povRight().whileTrue(new CoralExpire(pince, bougie)); manette1.leftTrigger().whileTrue(new DepartPince(pince)); manette1.povDown().whileTrue(new Algue_inspire(pince,bougie)); @@ -141,8 +140,8 @@ public class RobotContainer { manette2.x().whileTrue(new ExpireCorail(requin, bougie)); //limelight - manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); - manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY)); + // manette2.a().whileTrue(new Forme3(limelight3,drivetrain,manette1::getLeftX,manette1::getLeftY)); + // manette2.b().whileTrue(new AprilTag3(limelight3, drivetrain, manette1::getLeftX, manette1::getLeftY)); //Pince manuel pince.setDefaultCommand(new RunCommand(()->{ @@ -156,6 +155,7 @@ public class RobotContainer { //Reset encodeur manette2.start().whileTrue(new reset(elevateur, pince, requin)); + drivetrain.registerTelemetry(logger::telemeterize); } public Command getAutonomousCommand() {