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

mode auto dans subsystem fini

Browse Source
This commit is contained in:
Antoine PerreaultE
2025-12-08 18:29:44 -05:00
parent cf2580ca85
commit 0f9a71c375
4 changed files with 111 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
simgui-ds.json
View File

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

1
src/main/java/frc/robot/commands/Limelight/AprilTag3.java
View File

@@ -76,6 +76,7 @@ public class AprilTag3 extends Command {
withRotationalRate(tx/20). withRotationalRate(tx/20).
withVelocityX(2-botx). withVelocityX(2-botx).
withVelocityY(2-boty)); withVelocityY(2-boty));
} }
} }
else{ else{

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

@@ -1,9 +1,12 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import static edu.wpi.first.units.Units.MetersPerSecond;
import java.util.function.Supplier; import java.util.function.Supplier;
import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.hardware.Pigeon2;
import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants;
import com.ctre.phoenix6.swerve.SwerveRequest; import com.ctre.phoenix6.swerve.SwerveRequest;
import com.pathplanner.lib.auto.AutoBuilder; 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.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d; 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.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.wpilibj.DriverStation; 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.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.Subsystem;
import frc.robot.TunerConstants.TunerConstants;
import frc.robot.TunerConstants.TunerConstants.TunerSwerveDrivetrain; import frc.robot.TunerConstants.TunerConstants.TunerSwerveDrivetrain;
/** /**
@@ -44,51 +52,71 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
private SwerveDriveOdometry odometry; private SwerveDriveOdometry odometry;
private SwerveDrivePoseEstimator poseEstimator; private SwerveDrivePoseEstimator poseEstimator;
private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); 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. */ /* 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.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation();
private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); 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. * Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p> * <p>
@@ -107,27 +135,43 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
if (Utils.isSimulation()) { if (Utils.isSimulation()) {
startSimThread(); 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. * Constructs a CTRE SwerveDrivetrain using the specified constants.
@@ -151,7 +195,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
if (Utils.isSimulation()) { if (Utils.isSimulation()) {
startSimThread(); startSimThread();
} }
// configureAutoBuilder(); // configureAutoBuilder();
} }
/** /**
@@ -184,7 +228,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
if (Utils.isSimulation()) { if (Utils.isSimulation()) {
startSimThread(); startSimThread();
} }
configureAutoBuilder(); // configureAutoBuilder();
} }

8
vendordeps/PathplannerLib-2025.2.3.json → vendordeps/PathplannerLib-2025.2.7.json
View File

@@ -1,7 +1,7 @@
{ {
"fileName": "PathplannerLib-2025.2.3.json", "fileName": "PathplannerLib-2025.2.7.json",
"name": "PathplannerLib", "name": "PathplannerLib",
"version": "2025.2.3", "version": "2025.2.7",
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
"frcYear": "2025", "frcYear": "2025",
"mavenUrls": [ "mavenUrls": [
@@ -12,7 +12,7 @@
{ {
"groupId": "com.pathplanner.lib", "groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-java", "artifactId": "PathplannerLib-java",
"version": "2025.2.3" "version": "2025.2.7"
} }
], ],
"jniDependencies": [], "jniDependencies": [],
@@ -20,7 +20,7 @@
{ {
"groupId": "com.pathplanner.lib", "groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-cpp", "artifactId": "PathplannerLib-cpp",
"version": "2025.2.3", "version": "2025.2.7",
"libName": "PathplannerLib", "libName": "PathplannerLib",
"headerClassifier": "headers", "headerClassifier": "headers",
"sharedLibrary": false, "sharedLibrary": false,