Team_2648/2025_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

drivetrain odometry -> pose estimator

Browse Source
This commit is contained in:
Tylr-J42 2025-01-30 04:14:57 -05:00
parent 3af046f058
commit 89c1914d11
2 changed files with 15 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -13,11 +13,11 @@ import com.studica.frc.AHRS;
import com.studica.frc.AHRS.NavXComType; import com.studica.frc.AHRS.NavXComType;
import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.MathUtil;
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.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
@ -38,7 +38,7 @@ public class Drivetrain extends SubsystemBase {
private AHRS ahrs; private AHRS ahrs;
// Odometry class for tracking robot pose // Odometry class for tracking robot pose
private SwerveDriveOdometry m_odometry; private SwerveDrivePoseEstimator m_estimator;
/** Creates a new DriveSubsystem. */ /** Creates a new DriveSubsystem. */
public Drivetrain() { public Drivetrain() {
@ -68,7 +68,7 @@ public class Drivetrain extends SubsystemBase {
ahrs = new AHRS(NavXComType.kMXP_SPI); ahrs = new AHRS(NavXComType.kMXP_SPI);
m_odometry = new SwerveDriveOdometry( m_estimator = new SwerveDrivePoseEstimator(
DrivetrainConstants.kDriveKinematics, DrivetrainConstants.kDriveKinematics,
Rotation2d.fromDegrees(ahrs.getAngle()), Rotation2d.fromDegrees(ahrs.getAngle()),
new SwerveModulePosition[] { new SwerveModulePosition[] {
@ -76,7 +76,9 @@ public class Drivetrain extends SubsystemBase {
m_frontRight.getPosition(), m_frontRight.getPosition(),
m_rearLeft.getPosition(), m_rearLeft.getPosition(),
m_rearRight.getPosition() m_rearRight.getPosition()
}); },
new Pose2d()
);
AutoBuilder.configure( AutoBuilder.configure(
this::getPose, this::getPose,
@ -99,7 +101,7 @@ public class Drivetrain extends SubsystemBase {
@Override @Override
public void periodic() { public void periodic() {
// Update the odometry in the periodic block // Update the odometry in the periodic block
m_odometry.update( m_estimator.update(
Rotation2d.fromDegrees(getGyroValue()), Rotation2d.fromDegrees(getGyroValue()),
new SwerveModulePosition[] { new SwerveModulePosition[] {
m_frontLeft.getPosition(), m_frontLeft.getPosition(),
@ -132,7 +134,7 @@ public class Drivetrain extends SubsystemBase {
* @return The pose. * @return The pose.
*/ */
public Pose2d getPose() { public Pose2d getPose() {
return m_odometry.getPoseMeters(); return m_estimator.getEstimatedPosition();
} }
/** /**
@ -141,14 +143,7 @@ public class Drivetrain extends SubsystemBase {
* @param pose The pose to which to set the odometry. * @param pose The pose to which to set the odometry.
*/ */
public void resetOdometry(Pose2d pose) { public void resetOdometry(Pose2d pose) {
m_odometry.resetPosition( m_estimator.resetPose(
Rotation2d.fromDegrees(getGyroValue()),
new SwerveModulePosition[] {
m_frontLeft.getPosition(),
m_frontRight.getPosition(),
m_rearLeft.getPosition(),
m_rearRight.getPosition()
},
pose pose
); );
} }
@ -257,4 +252,8 @@ public class Drivetrain extends SubsystemBase {
public double getTurnRate() { public double getTurnRate() {
return ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0); return ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
} }
public void addVisionMeasurement(Pose2d pose, double timestamp){
m_estimator.addVisionMeasurement(pose, timestamp);
}
} }

4
src/main/java/frc/robot/subsystems/TylerVision.java → src/main/java/frc/robot/subsystems/Vision.java
View File

@ -5,7 +5,7 @@ import edu.wpi.first.networktables.DoubleSubscriber;
import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.NetworkTableInstance;
public class TylerVision{ public class Vision{
private DoubleSubscriber cam1GlobalPose; private DoubleSubscriber cam1GlobalPose;
private DoubleSubscriber cam1ClosestTag; private DoubleSubscriber cam1ClosestTag;
@ -13,7 +13,7 @@ public class TylerVision{
private DoubleSubscriber framerate; private DoubleSubscriber framerate;
public TylerVision(){ public Vision(){
NetworkTableInstance inst = NetworkTableInstance.getDefault(); NetworkTableInstance inst = NetworkTableInstance.getDefault();
NetworkTable visionTable = inst.getTable("Fiducial"); NetworkTable visionTable = inst.getTable("Fiducial");