From 0f9a71c3759c2a3aa13f8147cd3eff45d83778f6 Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Mon, 8 Dec 2025 18:29:44 -0500 Subject: [PATCH] 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,