mode auto dans subsystem fini

simgui-ds.json

@@ -1,7 +1,7 @@
 {
-  "System Joysticks": {
+  "Joysticks": {
     "window": {
-      "enabled": false
+      "visible": false
     }
   },
   "keyboardJoysticks": [

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{

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) {
+    // private void configureAutoBuilder() {
-        getState().Pose = newPose; // ou appelle setPose(newPose) selon ton code
+    //     try {
-    }//ca marche pas
+    //         var config = RobotConfig.fromGUISettings();
-    private void configureAutoBuilder() {
+    //         AutoBuilder.configure(  
-        try {
+    //             () -> getState().Pose,   // Supplier of current robot pose
-            var config = RobotConfig.fromGUISettings();
+    //             this::resetPose,         // Consumer for seeding pose against auto
-            AutoBuilder.configure(  
+    //             () -> getState().Speeds, // Supplier of current robot speeds
-                () -> getState().Pose,   // Supplier of current robot pose
-                this::resetOdometry,         // Consumer for seeding pose against auto
-                () -> getState().Speeds, // Supplier of current robot speeds
                 
-                // Consumer of ChassisSpeeds and feedforwards to drive the robot
+    //             // Consumer of ChassisSpeeds and feedforwards to drive the robot
-                (speeds, feedforwards) -> setControl(
+    //             (speeds, feedforwards) -> setControl(
-                    m_pathApplyRobotSpeeds.withSpeeds(speeds)
+    //                 m_pathApplyRobotSpeeds.withSpeeds(speeds)
-                        .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesXNewtons())
+    //                     .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesXNewtons())
-                        .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesYNewtons())
+    //                     .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesYNewtons())
-                ),
+    //             ),
-                new PPHolonomicDriveController(
+    //             new PPHolonomicDriveController(
-                    // PID constants for translation
+    //                 // PID constants for translation
-                    new PIDConstants(5, 0, 0),
+    //                 new PIDConstants(5, 0, 0),
-                    // // PID constants for rotation
+    //                 // // PID constants for rotation
-                    // new PIDConstants(7.9735, 0, 0.038499)
+    //                 // new PIDConstants(7.9735, 0, 0.038499)
-                    // PID constants for rotation
+    //                 // PID constants for rotation
-                    new PIDConstants(5,0,0)
+    //                 new PIDConstants(5,0,0)
-                ),
+    //             ),
-                config,
+    //             config,
-                // Assume the path needs to be flipped for Red vs Blue, this is normally the case
+    //             // Assume the path needs to be flipped for Red vs Blue, this is normally the case
-                () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
+    //             () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
-                this // Subsystem for requirements
+    //             this // Subsystem for requirements
                 
-            );
+    //         );
-        } catch (Exception ex) {
+    //     } catch (Exception ex) {
-            DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
+    //         DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
-        }
+    //     }
-          PPHolonomicDriveController.overrideRotationFeedback(()->{
+    //       PPHolonomicDriveController.overrideRotationFeedback(()->{
-             return 0;
+    //          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.
      * <p>
@@ -107,27 +135,43 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         if (Utils.isSimulation()) {
             startSimThread();
         }
-        configureAutoBuilder();
+        RobotConfig config;
-    }
+        System.out.println("avant le mode auto est genere");
-    // public void resetPose(Pose2d pose) {
+        try{
-    // // Reset gyro to match starting rotation
+        config = RobotConfig.fromGUISettings();
-    // pigeon2.setYaw(pose.getRotation().getDegrees());
+        SwerveRequest.ApplyRobotSpeeds driveAuto = new SwerveRequest.ApplyRobotSpeeds().withDriveRequestType(DriveRequestType.OpenLoopVoltage);
-
+        // Configure AutoBuilder last
-    // // Reset odometry
+        AutoBuilder.configure(
-    // m_odometry.resetPosition(
+                () -> getState().Pose, // Robot pose supplier
-    //     pose.getRotation(),
+                this::resetPose, // Method to reset odometry (will be called if your auto has a starting pose)
-    //     getState().ModulePositions,
+                () -> getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-    //     pose
+                (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
-    public void resetOdometry(Pose2d pose) {
+                        new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
-        odometry = new SwerveDriveOdometry(getKinematics(), kBlueAlliancePerspectiveRotation, getState().ModulePositions);
+                ),
-        odometry.resetPosition(
+                config, // The robot configuration
-            getPigeon2().getRotation2d(),
+                () -> {
-            getState().ModulePositions,
+                  // Boolean supplier that controls when the path will be mirrored for the red alliance
-            pose
+                  // 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();
     }
 
     

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,