PLS5618/Reefscape-2025
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

meillieur subsytem swerve (pas tent)

Browse Source
This commit is contained in:
Antoine PerreaultE
2025-12-08 19:23:02 -05:00
parent 0f9a71c375
commit 5e9f2f5244
5 changed files with 146 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
src/main/java/frc/robot/RobotContainer.java
View File

@@ -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() {

112
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
View File

@@ -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();
}