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

path palnner peut-etre

Browse Source
This commit is contained in:
Samuel
2025-12-01 18:59:04 -05:00
parent 9911ae781a
commit b0272a5640
1 changed files with 30 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -2,6 +2,7 @@ package frc.robot.subsystems;
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.SwerveModuleConstants;
import com.ctre.phoenix6.swerve.SwerveRequest;
@@ -10,7 +11,10 @@ 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.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.wpilibj.DriverStation;
@@ -30,14 +34,15 @@ 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();
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
@@ -51,7 +56,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
AutoBuilder.configure(
() -> getState().Pose, // Supplier of current robot pose
this::resetPose, // Consumer for seeding pose against auto
this::resetOdometry, // Consumer for seeding pose against auto
() -> getState().Speeds, // Supplier of current robot speeds
// Consumer of ChassisSpeeds and feedforwards to drive the robot
@@ -100,9 +105,28 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
if (Utils.isSimulation()) {
startSimThread();
}
//configureAutoBuilder();
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
);
this.getPigeon2().setYaw(pose.getRotation().getDegrees());
}
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p>
@@ -125,7 +149,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
if (Utils.isSimulation()) {
startSimThread();
}
// configureAutoBuilder();
configureAutoBuilder();
}
/**
@@ -158,7 +182,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
if (Utils.isSimulation()) {
startSimThread();
}
//configureAutoBuilder();
configureAutoBuilder();
}